Why can't my robot arm move to specific positions?
Hi, I am using MoveIT with ROS to try to move my robot to a pre defined (hardcoded) position. The problem that I have is that the "easiest" to reach targets cannot be reached, and "harder" targets can be reached. The start state of the robot looks like this:
I have drawn a red box where I want to be able to reach targets. Now, I am using move_group from MoveIT (C++) like this:
int main(int argc, char **argv)
{
ros::init(argc, argv, "cpp_arm");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const string PLANNING_GROUP = "arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup *joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
ROS_INFO_NAMED("arm", "Reference frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("gripper", "End effector link: %s", move_group.getEndEffectorLink().c_str());
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = -0.2;
target_pose1.position.y = 0.1;
target_pose1.position.z = 0.60;
move_group.setPoseTarget(target_pose1);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = move_group.plan(my_plan);
ROS_INFO_NAMED("arm", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group.move();
}
This results in the robot to move to the specific location:
When you look at this video, you can see the robot twists around itself and move in a strange way to its location.
(ignore the stuttering, its an camera issue):
Now, because of this strange movement ( I think ) the robot cannot reach simple positions in front or behind it like this:
And this:
I have used the same positions to move to as where the cubes are located at but it cannot reach them. I am getting the following erros:
Fail: ABORTED: No motion plan found. No execution attempted
And
RRTConnect: Unable to sample any valid states for goal tree
This is my URDF file:
<robot
name="robotarm">
<link
name="base_link">
<inertial>
<origin
xyz="-8.98316883899366E-21 0.0371833197218132 5.84804291418487E-18"
rpy="0 0 0" />
<mass
value="0.736649284793532" />
<inertia
ixx="0.00136014146084527"
ixy="1.34419615815924E-21"
ixz="-1.66676893429435E-19"
iyy="0.00201173713380681"
iyz="9.88817735312407E-20"
izz="0.00136014146084527" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="link_bescherming_voet">
<inertial>
<origin
xyz="1.89715073762982E-17 0.0246076878833881 -1.85713732548366E-17"
rpy="0 0 0" />
<mass
value="0.975479811968255" />
<inertia
ixx="0.00283902632376722"
ixy="-1.66823701635174E-19"
ixz="-2.59570718767664E-08"
iyy="0.0055247670044155"
iyz="8.3179086062007E-19"
izz="0.00402668154368391" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_bescherming_voet.STL" />
</geometry>
<material
name="">
<color
rgba="0.501960784313725 0.501960784313725 0.501960784313725 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_bescherming_voet.STL" />
</geometry>
</collision>
</link>
<joint
name="bescherming_voet"
type="revolute">
<origin
xyz="0 0 0.08"
rpy="1.5707963267949 6.12291108317084E-17 1.56451775600263" />
<parent
link="base_link" />
<child
link="link_bescherming_voet" />
<axis
xyz="0 1 0" />
<limit
lower="-3.1415926535898"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="link_onderstuk">
<inertial>
<origin
xyz="2.77555756156289E-16 0.0370359598710503 0.00249745962155625"
rpy="0 0 0" />
<mass
value="0.727126641555552" />
<inertia
ixx="0.00332215160948305"
ixy="-1.60630771451646E-17"
ixz="-3.59377738046309E-20"
iyy="0.00164734137380899"
iyz="-1.37219337455197E-19"
izz="0.00191216507803197" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_onderstuk.STL" />
</geometry>
<material
name="">
<color
rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_onderstuk.STL" />
</geometry>
</collision>
</link>
<joint
name="onderstuk_robotarm"
type="revolute">
<origin
xyz="0 0.061 0"
rpy="-1.57079632679489 1.66533453693774E-14 1.5707963267949" />
<parent
link="link_bescherming_voet" />
<child
link="link_onderstuk" />
<axis
xyz="0 0 1" />
<limit
lower="-3.1415926535898"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="link_middenstuk">
<inertial>
<origin
xyz="0.0171121874832891 0.0914745896991612 0.00596722744564532"
rpy="0 0 0" />
<mass
value="0.342780601633154" />
<inertia
ixx="0.00209352219888755"
ixy="8.61259300002377E-10"
ixz="-2.6521686422925E-10"
iyy="0.0001513360619975"
iyz="0.000121720831518031"
izz="0.00204368119640363" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_middenstuk.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_middenstuk.STL" />
</geometry>
</collision>
</link>
<joint
name="middenstuk_robotarm"
type="continuous">
<origin
xyz="0 0.143055389944023 -0.0146160189168883"
rpy="3.14159265358941 -1.55832909924767 -3.14159265358942" />
<parent
link="link_onderstuk" />
<child
link="link_middenstuk" />
<axis
xyz="0 -1 0" />
</joint>
<link
name="link_tussenstuk">
<inertial>
<origin
xyz="0.042225186109333 -0.0276464688445403 0.00221825165528444"
rpy="0 0 0" />
<mass
value="0.0146929182738881" />
<inertia
ixx="9.79920835370967E-06"
ixy="5.1351372427292E-21"
ixz="-3.45430623802144E-21"
iyy="1.06327575004056E-05"
iyz="2.17041852976258E-20"
izz="1.65869261432896E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_tussenstuk.STL" />
</geometry>
<material
name="">
<color
rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_tussenstuk.STL" />
</geometry>
</collision>
</link>
<joint
name="tussenstuk_robotarm"
type="continuous">
<origin
xyz="-0.0251130375427305 0.209723130216404 0.000313106175830219"
rpy="-1.56474174182987 -5.74540415243519E-15 5.49300188668056E-15" />
<parent
link="link_middenstuk" />
<child
link="link_tussenstuk" />
<axis
xyz="1 0 0" />
</joint>
<link
name="link_bovenstuk">
<inertial>
<origin
xyz="0.0423548102941489 -0.00892681385474869 -0.0709623288789762"
rpy="0 0 0" />
<mass
value="0.258131973119121" />
<inertia
ixx="0.000644471337323276"
ixy="-2.84604977136333E-10"
ixz="-6.36341097582589E-10"
iyy="0.000613223892148524"
iyz="7.98343530423762E-05"
izz="9.86664163020403E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_bovenstuk.STL" />
</geometry>
<material
name="">
<color
rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_bovenstuk.STL" />
</geometry>
</collision>
</link>
<joint
name="bovenstuk_robotarm"
type="continuous">
<origin
xyz="-0.000146563213221401 -0.0520202004343439 0.000435282046611418"
rpy="-1.5707963267949 0.00863982311053207 2.11297205627931E-15" />
<parent
link="link_tussenstuk" />
<child
link="link_bovenstuk" />
<axis
xyz="0 0 1" />
</joint>
<link
name="link_montage_grijper">
<inertial>
<origin
xyz="0.0427823459515499 0.0180745023432853 -0.00052810086084093"
rpy="0 0 0" />
<mass
value="0.0085410615680975" />
<inertia
ixx="2.38414878122796E-06"
ixy="-8.57414063941841E-21"
ixz="6.9929848985233E-21"
iyy="4.16919134966305E-06"
iyz="-1.19597391878097E-21"
izz="4.36342650764439E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_montage_grijper.STL" />
</geometry>
<material
name="">
<color
rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_montage_grijper.STL" />
</geometry>
</collision>
</link>
<joint
name="montage_grijper"
type="continuous">
<origin
xyz="-0.000427582421852708 -0.00123451622302745 -0.147972004628097"
rpy="3.13722090754549 8.33708102554453E-15 1.19088766625808E-15" />
<parent
link="link_bovenstuk" />
<child
link="link_montage_grijper" />
<axis
xyz="1 0 0" />
</joint>
<link
name="link_plaat_grijper">
<inertial>
<origin
xyz="0.0506039311498872 0.0481900257607031 0.00509605662971782"
rpy="0 0 0" />
<mass
value="0.0277940281502333" />
<inertia
ixx="1.82462908586164E-05"
ixy="2.13515554343703E-08"
ixz="1.57777692671081E-08"
iyy="1.89031544868748E-05"
iyz="-4.82441967600682E-06"
izz="2.6372016559843E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_plaat_grijper.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_plaat_grijper.STL" />
</geometry>
</collision>
</link>
<joint
name="plaat_grijper"
type="fixed">
<origin
xyz="-0.00774743207443959 0.10353765267612 0.0247322402258109"
rpy="3.14159265358979 1.73472347597681E-18 5.11743425413158E-17" />
<parent
link="link_montage_grijper" />
<child
link="link_plaat_grijper" />
<axis
xyz="0 0 0" />
</joint>
<link
name="link_grijper_pootrechts">
<inertial>
<origin
xyz="-0.00101901145429764 0.0838468984925917 0.00416398156504004"
rpy="0 0 0" />
<mass
value="0.018945632742562" />
<inertia
ixx="5.64376783052975E-05"
ixy="1.49773197001328E-06"
ixz="5.61831466729789E-10"
iyy="3.9822210767515E-07"
iyz="-4.6228946458019E-08"
izz="5.65142908961964E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_grijper_pootrechts.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_grijper_pootrechts.STL" />
</geometry>
</collision>
</link>
<joint
name="grijper_pootrechts"
type="continuous">
<origin
xyz="0.0755297780259896 0.0433842684954152 0"
rpy="-3.65321922018991E-16 2.38871422642006E-15 3.11161228998964" />
<parent
link="link_plaat_grijper" />
<child
link="link_grijper_pootrechts" />
<axis
xyz="0 0 1" />
</joint>
<link
name="link_grijper_pootlinks">
<inertial>
<origin
xyz="-0.0509847753245112 0.0856968825103725 -0.00416398156503998"
rpy="0 0 0" />
<mass
value="0.018945632742562" />
<inertia
ixx="5.64376783052975E-05"
ixy="1.49773197001314E-06"
ixz="-5.6183146672926E-10"
iyy="3.98222107675142E-07"
iyz="4.62289464580082E-08"
izz="5.65142908961964E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_grijper_pootlinks.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://robotarm/meshes/link_grijper_pootlinks.STL" />
</geometry>
</collision>
</link>
<joint
name="grijper_pootlinks"
type="continuous">
<origin
xyz="0.0755297780259896 0.0433842684954152 0"
rpy="-3.14159265358979 -3.10688974547446E-15 -0.0370081275083065" />
<parent
link="link_plaat_grijper" />
<child
link="link_grijper_pootlinks" />
<axis
xyz="0 0 -1" />
</joint>
</robot>
Why is this happening and how can I reach goal states in front of my robot?
Thanks in advance
Asked by nubdienub on 2017-06-27 05:46:59 UTC
Answers
Are you sure you want the end effector to move to orientation (x,y,z,w)=(0,0,0,1) (quaternion)?
Seeing your examples in front and behind the robot makes me wonder about that.
If you want position-only goals, you might want to use setPositionTarget instead.
Apart from this, it is a well-known issue that targets fail because of unreachable inverse kinematics requests. If this makes you feel any better: things are going to improve here quite a bit in the near future.
Until then: did you configure your setup to use TracIK for kinematics?
And are you really sure your robot can reach the position & orientation you request?
Asked by v4hn on 2017-06-27 09:00:05 UTC
Comments
And are you really sure your robot can reach the position & orientation you request?
this was what I was thinking as well: @nubdienub: is your robot a 6dof, or a 5dof?
Asked by gvdhoorn on 2017-06-27 09:36:03 UTC
Thanks for your answers, the robot is a 6dof robot. Also, I have tried to use setPositionTarget like this:
move_group.setPositionTarget(-0.2,0.1,0.60, "link_montage_grijper");
Which works, it moves to the location and also does not spin 360 degrees (only for about 45 degrees)
Asked by nubdienub on 2017-06-28 05:09:01 UTC
But I also tried this:
move_group.setPositionTarget(-0.2,0.3,0.60, "link_montage_grijper");
And this does not work. By default I have KDL solver but I just downloaded TracIK which cannot yet load: Kinematics solver could not be instantiated for joint group arm. I still have to figure this out.
Asked by nubdienub on 2017-06-28 05:10:32 UTC
Comments
This is a cross-post of this post on
moveit-users. Could I please ask you to not do that? At the very least it will lead to split (and duplicated) discussions about the same issue.Asked by gvdhoorn on 2017-06-27 08:21:00 UTC
And could I ask you to attach your images directly to the post? I've given you enough karma to do that.
Thanks.
Asked by gvdhoorn on 2017-06-27 08:21:48 UTC
Please also paste your urdf in the question. Use the Preformatted Text button (the one with
101010on it) to format it properly.Asked by gvdhoorn on 2017-06-27 08:23:28 UTC
gvdhoorn: such questions are mostly ignored on
moveit-usersanyway. The list is mostly for announcements.Asked by v4hn on 2017-06-27 08:49:12 UTC
Could be, but I believe it's always better to be explicit about that, instead of staying silent and hoping ppl pick up on it on their own.
Asked by gvdhoorn on 2017-06-27 09:09:04 UTC