Robotic System Toolbox workspaceGoalRegion prevents me to make a right Pick and Place with a robot manipualtor, surrounded by collision object in the scenario.

I'm using the Robotic System Toolbox in order to plan the path of a Robotic Manipulator in a scenario full of collision objects. I'm using the function manipulatorRRT, then in the function 'plan' it asks me to put the start and goal configurations of the robot. I'm using the function 'workspaceGoalRegion' instead of giving the Joint Configuration matrix, because using 'GoalRegion.ReferencePose=trvec2tform[ x y z ]' I can calculate the Joint configuration of the robot starting from a point I want to reach in the space.

The problem is that it works until a collision object is in front of the robot. In that situation matlab starts a neverending loop and the programme never stops.

Here is the script I am talking about:

scenario = robotScenario(UpdateRate=10);

addMesh(scenario,"Box",Position=[0.35 0 0.2],Size=[0.5 0.9 0.05],Color=[0.7 0.7 0.7],Collision="mesh")

addMesh(scenario,"Box",Position=[0 -0.6 0.2],Size=[1.3 0.4 0.235],Color=[0.7 0.7 0.7],Collision="mesh")

addMesh(scenario,"Box",Position=[0.3 -0.25 0.4],Size=[0.8 0.03 0.35],Color=[0.7 0.7 0.7],Collision="mesh")

addMesh(scenario,"Box",Position=[0.52 0 0.278],Size=[0.06 0.06 0.1],Color=[1 0.5 0.25],Collision="mesh")

addMesh(scenario,"Box",Position=[0.4 -0.1 0.2758],Size=[0.06 0.06 0.1],Color=[1 0.5 0.25],Collision="mesh")

visuals = "on";

collisions = "off";

show3D(scenario,Visuals=visuals,Collisions=collisions);

title("Scene with collision object-based static meshes")

view(81,19)

light

franka = loadrobot("frankaEmikaPanda");

robot = robotPlatform("Manipulator",scenario,RigidBodyTree=franka,ReferenceFrame="ENU");

initialConfig = homeConfiguration(robot.RigidBodyTree);

initialConfig(7) = 0.65;

initialConfig(6) = 0.75;

boxToMove = robotPlatform("Box",scenario,Collision="mesh",InitialBasePosition=[0.5 0.15 0.278]);

updateMesh(boxToMove,"Cuboid",Collision="mesh",Size=[0.06 0.06 0.1],Color=[0.9 0.0 0.0]);

setup(scenario)

move(robot,"joint",initialConfig)

ax = show3D(scenario,Visuals=visuals,Collisions=collisions);

title("Scene with static meshes and robot platform")

view(81,19)

light

goalRegion = workspaceGoalRegion(franka.BodyNames{end});

goalRegion.ReferencePose=trvec2tform([0.5 0.15 0.278+0.05]);

goalRegion.Bounds(1, :) = [0 0]; % X Bounds

goalRegion.Bounds(2, :) = [0 0]; % Y Bounds

goalRegion.Bounds(4, :) = [-pi/2 pi/2];

goalRegion.ReferencePose=goalRegion.ReferencePose*eul2tform([0 pi 0],"ZYX");

pickUpConfig=goalRegion;

%%

planner = manipulatorRRT(robot.RigidBodyTree,scenario.CollisionMeshes);

planner.MaxConnectionDistance = 0.6;

planner.ValidationDistance = 0.01;

planner.SkippedSelfCollisions = "parent";

planner.IgnoreSelfCollision = true;

rng("default")

path = plan(planner,initialConfig,pickUpConfig);

% Number of interpolations between each adjacent configuration.

numState = 10;

interpStates = interpolate(planner,path,numState);

show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

title("Approaching towards pick-up location")

for idx = 1:size(interpStates,1)

% Get current joint configuration.

jointConfig = interpStates(idx,:);

% Move manipulator with current joint configuration.

move(robot,"joint",jointConfig)

% Use fast update to move platform visualization frames.

show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

%Draw trajectory.

drawnow

% Advance scenario simulation time.

advance(scenario);

end

attach(robot,"Box","panda_hand",ChildToParentTransform=trvec2tform([0.0 0.0 0.1]))

show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

title("Picked-up target box")

startConfig=path(end,:);

goalRegion.ReferencePose=trvec2tform([0.4 -0.6 0.4]);

goalRegion.ReferencePose=goalRegion.ReferencePose*eul2tform([0 pi 0],"ZYX");

dropConfig=goalRegion;

planner = manipulatorRRT(robot.RigidBodyTree,scenario.CollisionMeshes);

planner.MaxConnectionDistance = 0.6;

planner.ValidationDistance = 0.01;

planner.SkippedSelfCollisions = "parent";

planner.IgnoreSelfCollision = true;

path = plan(planner,startConfig,dropConfig);

interpStates = interpolate(planner,path,numState);

% Visualize scene

show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

title("Approaching towards drop location")

for idx = 1:size(interpStates,1)

% Get current joint configuration.

jointConfig = interpStates(idx,:);

% Move manipulator with current joint configuration.

move(robot,"joint",jointConfig)

% Use fast update to move platform visualization frames.

show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

%Draw trajectory.

drawnow

% Advance scenario simulation time.

advance(scenario);

end

detach(robot)

show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

title("Placed target box at drop location")

read(robot,"joint")

Answer 1

[Karsh Tharyani]

Hi @Alessandro Di Cosola,

It seems like the IK solutions which are being considered as possible goal joint configuration states by planner manipulatorRRT for the robot are all in collision hence the planner is deeming this to be an impossible problem. I believe that is because of the default initial guess for a joint configuration being used internally by the planner which is the home configuration of the robot. As a workaround you could pass a goal joint configuration explicitly by calling inverseKinematics on a sampled pose from the goalRegion (by calling the "sample" function of a workspaceGoalRegion). You could wrap this up in a "while loop" to find a collision free goal joint configuration by sampling from the goal region and passing that to manipulatorRRT/plan.

I have conveyed an enhancement to allow a user-specified initial guess for planning to workspace goal regions using manipulatorRRT the appropriate development team at MathWorks, and should this be implemented in a future release of MATLAB you should be notified.

Hope that helps,

Karsh

[Karsh Tharyani]

Another possibility is to explicitly change the home joint position of the joints in the robot's rigidBodyTree to ultimately change the homeConfiguration.