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")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") 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

Cart

Cart