Robotic System Toolbox workspaceGoalRegion prevents me to make a right Pick and Place with a robot manipualtor, surrounded by collision object in the scenario.
1 visualizzazione (ultimi 30 giorni)
Mostra commenti meno recenti
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")
0 Commenti
Risposte (1)
Karsh Tharyani
il 13 Giu 2024
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
1 Commento
Karsh Tharyani
il 13 Giu 2024
Another possibility is to explicitly change the home joint position of the joints in the robot's rigidBodyTree to ultimately change the homeConfiguration.
Vedere anche
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!