Using moveit with 5-DoF arm

[Steffen P]

Hi all,

has anyone used moveit with a 5-DoF arm for pick and place tasks?

It always fails to grasp objects because of the missing DoF I think. 

(Manipulation plan failed at stage 'reachable & valid pose filter' on thread 1)

So whats the best (easiest) way to get this to work?

I have already tried to call move_group.pickup() with a grasp parameter I have calculated before to be reachable by the arm.

Back in Fuerte using arm_navigation I used a simple grasp planner made for my arm.

Is there a way in moveit to use this planner?

Thank you for your help!

[Michael Ferguson]

Steffen,

I recently got the TurtleBot Arm working with MoveIt. This arm is 4-dof, but I added a fake 5-th dof by putting a virtual joint between the gripper_link and the wrist_flex joint. I then generated an IKFast kinematics plugin for the arm, since I found no way to properly tell KDL kinematics plugin that the arm has less than 6DOF. It is still a bit of work in progress, but I've uploaded everything here: https://github.com/turtlebot/turtlebot_arm

Generating an ikfast plugin is generally pretty easy, see this repo https://github.com/ros-planning/moveit_ikfast You'll have to use the "translationdirection5d" instead of "transform6d" for the model. I also had problems with it not generating a solution until I had the end effector link oriented "correctly" for openrave.

-Fergs

[Steffen P]

Thank you for the reply. Yesterday I have looked into moveit_ikfast but wasn't sure if it would really help me.

With this translationdirection5d ik installed can you just call move_group.pickup("object") and you get a ik solution?
What exactly do you mean with " until I had the end effector link oriented "correctly" for openrave." Did you have to change the urdf or do you give a correct grasp as parameter into the pickup call?

Thank you very much again, I'll directly try ikfast again!

[Michael Ferguson]

Yes, with an IKfast solution, you should be able to call pickup and have things work.

As for "until I had the end effector oriented..." -- yes, I had to change the orientation of the last link in my chain, apparently there is some assumption built into openrave (I found a number of threads point to changing the "manipulator" direction if you were using openrave directly, but nothing specific to my exact problem. Rotating the frame fixed the issue, but in looking at it, it was really a poor original choice that might have been the culprit -- you'll know if you have a problem real fast, because ikfast.py will basically die with some horrible error if things aren't perfect.

-Fergs

[Martin Günther]

On Thu, 6 Jun 2013 00:33:24 -0700,
Michael Ferguson <mfe...@gmail.com> wrote:

> Yes, with an IKfast solution, you should be able to call pickup and
> have things work.
>
> As for "until I had the end effector oriented..." -- yes, I had to
> change the orientation of the last link in my chain, apparently there
> is some assumption built into openrave (I found a number of threads
> point to changing the "manipulator" direction if you were using
> openrave directly, but nothing specific to my exact problem. Rotating
> the frame fixed the issue, but in looking at it, it was really a poor
> original choice that might have been the culprit -- you'll know if
> you have a problem real fast, because ikfast.py will basically die
> with some horrible error if things aren't perfect.
>
> -Fergs

Hi Steffen and all,

Since you (Steffen) are also using a Katana arm, I thought I'd chime
in. I've been using the OpenRAVE IKFast 5Dof plugin since Diamondback
[1], but abandoned it at some point because it's made for the Katana
6M180 (with the straight gripper), and we have a 6M90A (with an angled
gripper), and the assumptions of the 5DoF IK don't hold for us.

IIRC, you have a 6M180, so the IKFast plugin should be perfect for
you. Collada descriptions for all Katana robots are here [2], plus a
README that's probably badly out of date (patches to both README and
Collada files are welcome!). I've never used Dave's moveit_ikfast, but
it looks awesome, so you should better start from the instructions
there [3].

Regarding the direction: I also had this issue back then, so this [4]
might be useful.

Let me know how it goes, maybe it gives me hope to try this path once
again for our Katana version. :-)

Cheers,
Martin

[1]: https://github.com/uos/katana_manipulation/tree/a4e040169456c93664a880cba4a3951d1c6c7ad7/katana_kinematics_constraint_aware

[2]: https://github.com/uos/katana_driver/tree/groovy/katana_description/collada

[3]: http://moveit.ros.org/wiki/Kinematics/IKFast

[4]: http://answers.ros.org/question/9988/openrave-ik-fails-to-find-a-solution-on-katana-6m90a-arm/

--
Dipl.-Inf. Martin Günther
Universität Osnabrück
Institut für Informatik
Albrechtstr. 28 (Raum 31/503)
D-49076 Osnabrück

Telefon: +49 (0)541 969 2434

http://www.inf.uos.de/mguenthe/

[Steffen P]

Thank you Martin and Fergs,

I have successfully generated the IKFast plugin and integrated it into my moveit package.

My problem now:

In moveit positional IK works but I have problems with the orientation.

I use a Katana 350 6m180 so the black gripper fingers are the correct one. The green gripper is the virtual one belonging to the interactive marker.

This is the one I can actualle move around.

Here are two screenshots from moveit demo.launch:

I can set a desired pose with the interactive markers but the solution calculated (black fingers) seems to be rotate 90 degrees around y axis.

So I have to somehow change the direction but I don't know how. I don't want to change the urdf.

[Michael Ferguson]

Steffen,

I found that I had the same problem with the interactive markers, but that if I sent requests to move it via the action interfaces (move_group, pick, place) everything works as expected. In other words, I think this is some sort of bug in the visualization for the RVIZ plugin, but it does not actually affect the planning going on underneath.

-Fergs

[Steffen P]

Unfortunately I have tested the move_group functions first and they all failed except if I use move_group->setPositionTarget().

mg->setRPYTarget(0,0,0) e.g. gives the following messages:

[ INFO] [1371472211.270960027]: Planning attempt 1 of at most 1

[ INFO] [1371472211.275607021]: Starting with 1 states

[ WARN] [1371472211.287290885]: More than 80% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?

[ INFO] [1371472211.612027496]: Orientation constraint violated for link 'katana_gripper_link'. Quaternion desired: 0.000000 0.000000 0.000000 1.000000, quaternion actual: 0.707208 0.000407 0.707006 0.000543, error: x=0.592196, y=1.570452, z=0.590853, tolerance: x=0.001000, y=0.001000, z=0.001000

[ERROR] [1371472211.845772696]: Unable to sample any valid states for goal tree

[ INFO] [1371472211.845870473]: Created 1 (1 start + 0 goal) states in 1 cells (1 start (1 on boundary) + 0 goal (0 on boundary))

[ INFO] [1371472211.845944038]: No solution found after 0.571223 seconds

[ INFO] [1371472211.846022397]: Unable to solve the planning problem

The error is so big that if I set the tolerance e.g. to 1.5 the orientation constraints are already fulfilled without moving.

Somehow I have the feeling that this is an orientation problem of the gripper frame or some gripper frame orientation assumptions made by ikfast.

When I tried the demo.launch yesterday and got this two differently orientated grippers I was confirmed in this feeling.

But if you say it's the same for you and you still can use move_group I don't know what to try next.

[Steffen P]

Dear Fergs,

I have been trying to get out Robot to work for over 1 month now.

My motivation slowly begins to fade away. 

I'm even thinking of going back to Fuerte.

But one last try:

You have posted your robots's repository but I would be really interested in the exact commands you used to generate the IKFast.

Which is your baselink and which your eelink link as ikfast.py parameters?

Thank you very much!

-Steffen

Am Mittwoch, 19. Juni 2013 22:44:42 UTC+2 schrieb Fergs:

[Ioan Sucan]

Steffen,

I am sorry to hear this is causing you so much trouble. We are actually working on improving how the KDL plugin works -- we will offer approximate solutions for IK requests, as indicated in this ticket:
https://github.com/ros-planning/moveit_core/issues/75

We have postponed this a bit, as it will be a breaking change (API) for the kinematics solvers, and we wanted to wait until after Darpa VRC.

Once this is done, I think your 5dof arm will behave better with KDL. So... if you have just one last try in you, maybe wait a bit until issue 75 mentioned above is fixed? :)

Ioan

[Steffen P]

Sorry, I am to stupid to use Google groups... here are the last posts from Ioan and Dave:

@Dave: Can you say something about the assumptions IKFast, especially the "translationdirection5d" one, has for the orientation of the gripper frame relative to the base frame or the orientation of the gripper frame relative to the gripper itself.

Things like gripping direction and the normal to the palm.

Ioan,

sounds good, is there already some code available?

I'm currently working/writing on my master thesis which is about robust pick and place but not working at all is far from "robust" ;-) 2 months left...

Steffen P:

If I use the IKFast plugin the marker issue as described in issue 75 is solved.

The positional IK works perfect. But I have orientation issues. 

There must be some orientation assumptions in IKFast translationdirection5d but there is no documentation.

---

Ioan Sucan:

Steffen, I am unfortunately not knowledgeable about IKFast at all.

Do you have any hints Dave? 

The changes I mentioned will likely only affect KDL, not IKFast.

---

Steffen P:

Yes, I would instantly switch back to KDL if it would work. 
Is there already some code written to support < 6 dof arms?

---

Ioan Sucan:

By next week, I expect this will happen.

--

Dave Coleman:

What is the issue with IKFast? I've banged my head against the wall over it for months, its a tricky beast to figure out.

[Michael Ferguson]

I worked off of http://moveit.ros.org/wiki/Kinematics/IKFast

Note, that I had to run "openrave0.8-robot.py" on the UNROUNDED .dae file, the rounded one causes crashes, I then use the rounded.dae file for the final export

$openrave0.8-robot.py arm.dae --info links

name                         index parents                     

---------------------------------------------------------------

base_link                    0                                 

arm_base_link                1     base_link                   

arm_shoulder_pan_servo_link  2     arm_base_link               

arm_shoulder_pan_link        3     arm_shoulder_pan_servo_link 

arm_shoulder_lift_servo_link 4     arm_shoulder_pan_link       

arm_shoulder_lift_link       5     arm_shoulder_lift_servo_link

arm_shoulder_F10_0_link      6     arm_shoulder_lift_link      

arm_shoulder_F10_1_link      7     arm_shoulder_F10_0_link     

arm_shoulder_F10_2_link      8     arm_shoulder_F10_1_link     

arm_shoulder_F3_0_link       9     arm_shoulder_F10_2_link     

arm_elbow_flex_servo_link    10    arm_shoulder_F3_0_link      

arm_elbow_flex_link          11    arm_elbow_flex_servo_link   

arm_elbow_F10_0_link         12    arm_elbow_flex_link         

arm_elbow_F10_1_link         13    arm_elbow_F10_0_link        

arm_elbow_F10_2_link         14    arm_elbow_F10_1_link        

arm_elbow_F3_0_link          15    arm_elbow_F10_2_link        

arm_wrist_flex_servo_link    16    arm_elbow_F3_0_link         

arm_wrist_flex_link          17    arm_wrist_flex_servo_link   

arm_wrist_F3_0_link          18    arm_wrist_flex_link         

gripper_servo_link           19    arm_wrist_F3_0_link         

gripper_active_link          20    gripper_servo_link          

gripper_active_finger_link   21    gripper_active_link         

gripper_static_link          22    gripper_servo_link          

gripper_static_finger_link   23    gripper_static_link         

gripper_link                 24    arm_wrist_flex_link         

---------------------------------------------------------------

name                         index parents                 

$ openrave0.8-robot.py arm.dae --info joints

name                          joint_index dof_index parent_link                  child_link                   mimic

-------------------------------------------------------------------------------------------------------------------

arm_shoulder_pan_joint        0           0         arm_shoulder_pan_servo_link  arm_shoulder_pan_link             

arm_shoulder_lift_joint       1           1         arm_shoulder_lift_servo_link arm_shoulder_lift_link            

arm_elbow_flex_joint          2           2         arm_elbow_flex_servo_link    arm_elbow_flex_link               

arm_wrist_flex_joint          3           3         arm_wrist_flex_servo_link    arm_wrist_flex_link               

gripper_joint                 4           4         gripper_servo_link           gripper_active_link               

gripper_link_joint            5           5         arm_wrist_flex_link          gripper_link                      

arm_base_joint                -1          -1        base_link                    arm_base_link                     

arm_shoulder_pan_servo_joint  -1          -1        arm_base_link                arm_shoulder_pan_servo_link       

arm_shoulder_lift_servo_joint -1          -1        arm_shoulder_pan_link        arm_shoulder_lift_servo_link      

arm_shoulder_F10_0_joint      -1          -1        arm_shoulder_lift_link       arm_shoulder_F10_0_link           

arm_shoulder_F10_1_joint      -1          -1        arm_shoulder_F10_0_link      arm_shoulder_F10_1_link           

arm_shoulder_F10_2_joint      -1          -1        arm_shoulder_F10_1_link      arm_shoulder_F10_2_link           

arm_shoulder_F3_0_joint       -1          -1        arm_shoulder_F10_2_link      arm_shoulder_F3_0_link            

arm_elbow_flex_servo_joint    -1          -1        arm_shoulder_F3_0_link       arm_elbow_flex_servo_link         

arm_elbow_F10_0_joint         -1          -1        arm_elbow_flex_link          arm_elbow_F10_0_link              

arm_elbow_F10_1_joint         -1          -1        arm_elbow_F10_0_link         arm_elbow_F10_1_link              

arm_elbow_F10_2_joint         -1          -1        arm_elbow_F10_1_link         arm_elbow_F10_2_link              

arm_elbow_F3_0_joint          -1          -1        arm_elbow_F10_2_link         arm_elbow_F3_0_link               

arm_wrist_flex_servo_joint    -1          -1        arm_elbow_F3_0_link          arm_wrist_flex_servo_link         

arm_wrist_F3_0_joint          -1          -1        arm_wrist_flex_link          arm_wrist_F3_0_link               

gripper_servo_joint           -1          -1        arm_wrist_F3_0_link          gripper_servo_link                

gripper_active_finger_joint   -1          -1        gripper_active_link          gripper_active_finger_link        

gripper_static_joint          -1          -1        gripper_servo_link           gripper_static_link               

gripper_static_finger_joint   -1          -1        gripper_static_link          gripper_static_finger_link        

-------------------------------------------------------------------------------------------------------------------

name                          joint_index dof_index parent_link                  child_link                   mimic

I then used base_link (0) as the baselink, and gripper_link (24) as the eelink (which corresponds to my SRDF and moveit config)

I then generated using:

/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py --robot=arm.rounded.dae --iktype=translationdirection5d --baselink=0 --eelink=24

And as I said, the interactive markers demo doesn't really work -- the orientation of the marker goes all wonky, but if I call move_group action, I get the results I want.

-Fergs

[Martin Günther]

Hi Steffen and all,

I'm pretty sure I solved it! (Scroll down to where the code begins for
the solution, I'm not very good at making a story short.)

Disclaimer: The IKFast plugin I generated is for arm_navigation on
Fuerte, but the solution below should apply to MoveIt as well.

I spent the last 3 days generating an IKFast plugin for the Katana
6M90a (once again; my first try was more than two years ago), this time
following this [1] tutorial. I ran into similar problems as Steffen
with his 6M180. The position works, but the rotation is off. Here's
a video of that:

http://vimeo.com/69235449

The vital hint is in the ComputeIk() doc in ikfast.h:
- ``eerot``
- For **Transform6D** it is 9 values for the 3x3 rotation matrix.
- For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction.
- For [...]

The IKFastKinematicsPlugin::solve() method in <my_robot>_<manipulator>_ikfast_plugin.cpp
calls ComputeIk() with a rotation matrix (as is expected for Transform6D),
but for TranslationDirection5D, ComputeIk() expects a vector.

With the following fix, everything works perfectly:

int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree)
{
// IKFast56/61
solutions_.Clear();

//KDL::Rotation rot = KDL::Rotation::RotY(M_PI/2);
KDL::Rotation orig = pose_frame.M;
KDL::Rotation mult = orig;//*rot;
-
- double vals[9];
- vals[0] = mult(0,0);
- vals[1] = mult(0,1);
- vals[2] = mult(0,2);
- vals[3] = mult(1,0);
- vals[4] = mult(1,1);
- vals[5] = mult(1,2);
- vals[6] = mult(2,0);
- vals[7] = mult(2,1);
- vals[8] = mult(2,2);
+ KDL::Vector direction = mult * KDL::Vector(0, 0, 1);

double trans[3];
trans[0] = pose_frame.p[0];//-.18;
trans[1] = pose_frame.p[1];
trans[2] = pose_frame.p[2];

// IKFast56/61
- ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions_);
+ ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions_);
return solutions_.GetNumSolutions();
}


Best wishes,
Martin

[1]: http://ros.org/wiki/Industrial/Tutorials/Create_a_Fast_IK_Solution

[Michael Ferguson]

Martin,

Awesome work, just tried this out on the turtlebot arm, and it makes a huge difference -- the interactive markers are now usable.

-Fergs

[Patrick Goebel]

This is very exiting since it suggests I might be able to get the
"LookAt3D" IK to work with a pan-and-tilt head. I'll give it a try this
weekend and report back. (I already tried it without your modification
and it did not work with the generated MoveIt plugin. Now I know why.)

Thanks Martin!

--patrick

[Martin Günther]

On Fri, 28 Jun 2013 06:25:51 -0700
Patrick Goebel <pat...@pirobot.org> wrote:

> This is very exiting since it suggests I might be able to get the
> "LookAt3D" IK to work with a pan-and-tilt head. I'll give it a try
> this weekend and report back. (I already tried it without your
> modification and it did not work with the generated MoveIt plugin.
> Now I know why.)
>
> Thanks Martin!

Nice to know this post is helping people! :-)

Not sure whether my fix will work for the LookAt3D IK, since the
ikfast.h comments for ComputeIk() don't mention it (make sure to read
that). Intuitively, I'd guess LookAt3D only requires a translation
(eetrans) and ignores eerot completely; that's something that's easy to
find out by looking into the generated code and check which elements of
eetrans/eerot are accessed. For TranslationDirection5D, it's only
eetrans[0-2] and eerot[0-2]; for Transform6D, it's eetrans[0-2] and
eerot[0-8].

Best,
Martin

[Dave Coleman]

Martin, well done! Can we get a pull request to add this fix to the main moveit_ikfast repo?

:: dave | 251.463.2345 c

[Steffen P]

Hello Martin,

this looks really good.

I was just fiddling around with the "rot" commented out in the code snipped. So I was pretty close ;-)

I first tried directly pasting your code into my ikfast_plugin.cpp but it was not getting better.

So I changed the KDL::Vector to (1,0,0) which let me move around with the interactive markers much better.

Why did you choose the (0,0,1) vector?

What does the KDL::Vector exactly mean? Is it the grasp direction in the gripper frame?

Which link did you choose as end effector link?

I currently "use katana_motor5_wrist_roll_link" but I think it's orientation is a pretty bad choice.

For our Katana the grasp direction is (0,0,-1).

Matrin, thank you very much for your post it really helps me a lot!

[Steffen P]

I have just noticed that Martin has pushed his changes to the https://github.com/uos/katana_driver repository.

This should answer my questions.

[Martin Günther]

Hi Steffen,

On Mon, 1 Jul 2013 02:52:00 -0700 (PDT),
Steffen P <s.pfi...@gmail.com> wrote:

> I first tried directly pasting your code into my ikfast_plugin.cpp
> but it was not getting better.
> So I changed the KDL::Vector to (1,0,0) which let me move around with
> the interactive markers much better.
> Why did you choose the (0,0,1) vector?
> What does the KDL::Vector exactly mean? Is it the grasp direction in
> the gripper frame?
>
> Which link did you choose as end effector link?
> I currently "use katana_motor5_wrist_roll_link" but I think it's
> orientation is a pretty bad choice.

These questions are all related, so I'll try to answer them together.
What I did is I modified the URDF so that the katana_gripper_tool_frame
link's origin is in the center of the gripper, and that its z axis
points in the direction of the fingers.

TranslationDirection5D seems to be defined as "x/y/z of desired and
achieved pose are equal, and z axis of desired and achieved pose point
into the same direction". That means rotation around the z axis is
allowed.

I experimented with different values for the <direction> tag in the
*.robot.xml, but the C++ code generated by IkFast was always the same, so
I assume it's always the z axis (and this is why I've hard coded
KDL::Vector(0, 0, 1)). If you find out that's not true, let me know. In
the end I ignored the .robot.xml file completely and used the .dae file
directly.

The katana_motor5_wrist_roll_link was also my first choice, but it
turns out it's a really bad one because its z axis coincides with
the motor5 joint axis, and since rotation around the z axis is allowed,
we completely lose the effect of moving one joint (motor5).

It turns out that z has to be oriented that way for our gripper because
this is the only axis we can't control (that's the missing DoF). For
your arm, it's different (see below).

What I'd do in your place is this:
* throw away the .robot.xml file
* edit your URDF so that your tool frame is between the fingers, but
the z axis is perpendicular to the fingers (hard to describe; if the
gripper was lying flat on the table, z should be pointing up).
* regenerate your .dae file (see tutorial)
* regenerate your ikfast file

If that works, you can still go back to making it work with your .robot.xml file.

I see you've already found my updates to the katana_driver repo;
there's also a matching branch called ikfast in the kurtana_robot repo.

FYI, here are the OpenRAVE commands used to generate the plugin code:

$ openrave-robot.py katana_450_6m90a.dae --info links
name index parents
-----------------------------------------------------------------
katana_base_link 0
katana_motor1_pan_link 1 katana_base_link
katana_motor2_lift_link 2 katana_motor1_pan_link
katana_motor3_lift_link 3 katana_motor2_lift_link
katana_motor4_lift_link 4 katana_motor3_lift_link
katana_motor5_wrist_roll_link 5 katana_motor4_lift_link
katana_gripper_link 6 katana_motor5_wrist_roll_link
katana_l_finger_link 7 katana_gripper_link
katana_r_finger_link 8 katana_gripper_link
katana_gripper_tool_frame 9 katana_motor5_wrist_roll_link
-----------------------------------------------------------------
name index parents

# generate katana_450_6m90a.dae (can also be found in katana_description/collada/)
rosrun xacro xacro.py $(rospack find katana_description)/urdf/katana_450_6m90a.urdf.xacro > katana_450_6m90a.urdf
rosrun collada_urdf urdf_to_collada katana_450_6m90a.urdf katana_450_6m90a.dae

# generate kurtana_arm_ikfast_solver.cpp (can also be found in katana_ikfast_kinematics_plugin/src/katana_450_6m90a_ikfast_plugin.cpp)
/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py --robot=katana_450_6m90a.dae --iktype=translationdirection5d --baselink=0 --eelink=9 --savefile=katana_450_6m90a_ikfast_plugin.cpp
# This lead to a "RuntimeError: maximum recursion depth exceeded", so I had to add
# sys.setrecursionlimit(100000)
# into /usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py

> For our Katana the grasp direction is (0,0,-1).

Where do you specify that? In the <direction> tag of the
katana*.robot.xml?


Cheers,
Martin

[Steffen P]

Hey Martin,
first thanks for the detailed description.
I saw the changes in your urdf and adapted mine and converted it to dae.
I now have it running and behaving well in rviz.
For rviz to behave correctly,  I had to change the parent frame of the gripper to the gripper tool frame (in the moveit assistant). Now the interactive markers display correctly.
For the first time I was able to programmatically set a pose:
Position 0.35 0 0.90
Orientation 0 0 0 1

But if I try the same pose just with the position's y value set to 0.05 it doesn't find a solution. But I'll try more  tomorrow.

Does this work for you?

I'll send you a pull request for the changes for our Katana as soon as it works.

>> For our Katana the grasp direction is (0,0,-1).

>Where do you specify that? In the <direction> tag of the
>katana*.robot.xml?

I never used the robot.xml. I just tried to describe the gripper direction in the wrist roll frame.

Thanks again!
I hope I can finally really start my master thesis tomorrow (after 4 months ; -) )

Greetz,
Steffen

[Martin Günther]

On Mon, 1 Jul 2013 19:21:26 +0200
Steffen P <s.pfi...@gmail.com> wrote:

> Hey Martin,
> first thanks for the detailed description.
> I saw the changes in your urdf and adapted mine and converted it to
> dae. I now have it running and behaving well in rviz.
> For rviz to behave correctly, I had to change the parent frame of the
> gripper to the gripper tool frame (in the moveit assistant). Now the
> interactive markers display correctly.
> For the first time I was able to programmatically set a pose:
> Position 0.35 0 0.90
> Orientation 0 0 0 1
>
> But if I try the same pose just with the position's y value set to
> 0.05 it doesn't find a solution. But I'll try more tomorrow.

Well, first of all the difference between the two poses is that the
first is actually reachable with your arm, the second is not: If y
is != 0, yaw can't be 0, but has to have a certain angle (something
like atan2(y, x)). BUT this is exactly where 5D IK should help: because
rotations around z are okay, the IK should figure out the correct yaw
angle automagically.

Do you have an RViz screenshot that shows the TF of the
katana_gripper_tool_frame? (To check where your Z axis goes).

A simpler explanation would be of course that the new position is
simply outside the workspace.

As a simple way to distinguish between those two possibilities, I
recommend the following. Put some debug output into your plugin like
this:

https://github.com/uos/katana_driver/commit/39156b893d6f5bc700f607dc87ba12f77d08f55d

Then set your logger level properly and move the gripper in RViz. You
should see the output. Now when you move the gripper first in the -x
direction and then in the y direction, you should see that the IK
figures out the yaw angle by itself. If not, probably the direction of
the tool frame's z axis was a bad choice.

Cheers,
Martin

[Steffen P]

Well, first of all the difference between the two poses is that the
first is actually reachable with your arm, the second is not: If y
is != 0, yaw can't be 0, but has to have a certain angle (something
like atan2(y, x)). BUT this is exactly where 5D IK should help: because
rotations around z are okay, the IK should figure out the correct yaw
angle automagically.

Yes, this is exactly what I intended to test with these poses. 

Do you have an RViz screenshot that shows the TF of the
katana_gripper_tool_frame? (To check where your Z axis goes).

A simpler explanation would be of course that the new position is
simply outside the workspace.

As a simple way to distinguish between those two possibilities, I
recommend the following. Put some debug output into your plugin like
this:

https://github.com/uos/katana_driver/commit/39156b893d6f5bc700f607dc87ba12f77d08f55d

Then set your logger level properly and move the gripper in RViz. You
should see the output. Now when you move the gripper first in the -x
direction and then in the y direction, you should see that the IK
figures out the yaw angle by itself. If not, probably the direction of
the tool frame's z axis was a bad choice.

Yes, in RVIZ I can nicely move the gripper around and it automatically adjusts the yaw angle.

I have just noticed that my ikfast_moveit_plugin.cpp looks different than your's:

Did you use the moveit ikfast script or do you currently use ikfast without moveit?

For example the getPositionIK prototype:

Mine:

bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,

                                           const std::vector<double> &ik_seed_state,

                                           std::vector<double> &solution,

                                           moveit_msgs::MoveItErrorCodes &error_code,

                                           bool lock_redundant_joints) const

Yours:

bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,

                                            const std::vector<double> &ik_seed_state,

                                            std::vector<double> &solution,

                                            int &error_code)

 {

[Martin Günther]

Hi Steffen,

On Wed, 3 Jul 2013 09:45:57 +0200,
Steffen P <s.pfi...@gmail.com> wrote:

> Yes, in RVIZ I can nicely move the gripper around and it automatically
> adjusts the yaw angle.

Your screenshot looks correct, too.

> I have just noticed that my ikfast_moveit_plugin.cpp looks different
> than your's:
> Did you use the moveit ikfast script or do you currently use ikfast
> without moveit?

I used the arm_navigation tutorial, since I've gone back to using
arm_navigation for now:

http://ros.org/wiki/Industrial/Tutorials/Create_a_Fast_IK_Solution/arm_navigation_plugin

Yesterday I've also gone through the moveit tutorial and added a moveit
plugin here, which should look much more similar to yours:

https://github.com/uos/katana_driver/tree/groovy/katana_moveit_ikfast_plugin

[Patrick Goebel]

So I am trying the LookAt3D IKFast solver with MoveIt! and thanks to Martin's notes, I was able to tweak the generated .cpp file to allow a goal specified as a translation representing the point in space to look at.  The resulting plugin builds fine and the .so file is created.  So my robot now has two move groups (right_arm and head) and two solvers (transform6d for the arm and lookat3d for the head).

However, when I try to launch the demo.launch file for my robot, I get the following fatal errors:

[ INFO] [1372858979.940463944]: No root joint specified. Assuming fixed joint
[ INFO] [1372858980.400225773]: head_pan_joint -2.9 2.9 1
[ INFO] [1372858980.400310319]: head_tilt_joint -2.9 2.9 1
[FATAL] [1372858980.409408785]: Joint numbers mismatch: URDF has 3 and IKFast has 2
[ERROR] [1372858980.409559244]: Kinematics solver of type 'pedestal_pi_no_gripper_right_arm_kinematics/IKFastKinematicsPlugin' could not be initialized for group 'right_arm'

I've double-checked my URDF file and it only has 2 non-fixed joints for the head group.  Also, if I edit my kinematics.yaml file and change the IK solver for the head to KDL instead of the IKFast lookat3d plugin, then demo.launch runs fine--both solvers are loaded without error but of course now I don't have the IKFast solver for the head group.

Any thoughts about what might be going on?

Thanks,
patrick

P.S.  My kinematics.yaml file looks like the following.  (If I un-comment  the first line and comment the second line to use the lookat3d solver for the head, I get the errors described above).

head:
  #kinematics_solver: pedestal_pi_no_gripper_head_kinematics/IKFastKinematicsPlugin
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_attempts: 3
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
right_arm:
  kinematics_solver: pedestal_pi_no_gripper_right_arm_kinematics/IKFastKinematicsPlugin
  kinematics_solver_attempts: 3
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05

[Michael Ferguson]

So one thing to take a look at is: is the base frame correct? (especially if you have a torso lift joint).

For the turtlebot_arm, I have a hack to hard code the base_frame (https://github.com/turtlebot/turtlebot_arm/commit/5c5b5ee8f0a8b34709698dfaca460dde1f4af57a) so that you can attach the arm to whatever model, and it just works (because my base_link selection for ikfast.py was not "base_link")

-Fergs

[Patrick Goebel]

Thanks Fergs,

I don't have a torso lift joint and I believe my base frame is correct (I just use the base_link frame).  I tried your hack anyway but I still got the same "joint count mismatch error".  I'm thinking there is a problem with the way the generated IKFast .cpp code is counting the joints in my use case so I'll look at that in more detail.

--patrick

[Steffen P]

Hi,

I made some progress.

It's now possible to use move() to reach any possible pose. (I use atan2 to calculate the yaw angle of the pose because ikfast 5D doesn't work as it should.)

But when it comes to pick() it get the following INFO/Error: (I use the latest moveit built from source)

[ INFO] [1373362520.026804792]: Planning attempt 1 of at most 1

[ INFO] [1373362520.028098617]: Added plan for pipeline 'pick'. Queue is now of size 1

[ INFO] [1373362520.030539748]: Adapted ik validity callback enabled

[DEBUG] [1373362520.030936925]: searchPositionIK

[DEBUG] [1373362520.031108391]: No need to search since no free params/redundant joints

[DEBUG] [1373362520.031264220]: getPositionIK

[DEBUG] [1373362520.031700347]: Found 4 solutions from IKFast

[DEBUG] [1373362520.032261399]: Sol 0: 2.232284e-01   8.100319e-01   1.514181e+00   2.612727e+00   -3.439713e-03   6.938757e-310

[DEBUG] [1373362520.032424666]: Not in limits! 3 value 2.61273 has limit: 1  being  -2.03331 to 1.87613

[DEBUG] [1373362520.032528558]: Sol 1: 2.232284e-01   2.033324e+00   -1.514181e+00   8.076587e-01   -3.439713e-03   5.365256e+199

[DEBUG] [1373362520.034713698]: Solution passes callback

[ INFO] [1373362520.034941420]: IK_sol.size(): 0 ik_joint_bijection (move_group) size: 5

move_group: /home/sp/groovy_catkin_workspace/src/overlay/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp:561: bool constraint_samplers::IKConstraintSampler::callIK(const Pose&, const IKCallbackFn&, double, robot_state::JointStateGroup*, bool): Assertion `ik_sol.size() == ik_joint_bijection.size()' failed.

[move_group-1] process has died [pid 32672, exit code -6, cmd /home/sp/groovy_catkin_workspace/devel/lib/moveit_ros_move_group/move_group __name:=move_group __log:=/home/sp/.ros/log/3586a5dc-e872-11e2-8285-e83935338487/move_group-1.log].

log file: /home/sp/.ros/log/3586a5dc-e872-11e2-8285-e83935338487/move_group-1*.log

The assertion failed INFO message seems strange to me. In the line above a solution was found, but the IK_sol.size() is 0.

Can someone please help me?

-Steffen

[Ioan Sucan]

Hello Steffen,

It looks like the call to searchPositionIK() in the IK plugin does not call .resize() on the solution vector. If you insert that call, that assertion should be met.

Ioan

[Steffen P]

I added some DEBUG to the end of getPositionIK, searchPositionIK and to the ikcallback:

The size seems to be ok there.

So I have no idea where it gets lost.

Seems that I have to use GDB...

[DEBUG] [1373368035.030899986]: Found 4 solutions from IKFast

[DEBUG] [1373368035.031120181]: Sol 0: 3.556939e-01   1.822744e+00   -1.739903e+00   2.219602e-01   -8.715480e-03   6.900110e-310

[DEBUG] [1373368035.031450943]: getPositionIK: size of solution5

[ INFO] [1373368035.031742924]: ikcallback: ik_sol.size 5 bij.size: 5

[DEBUG] [1373368035.032825317]: searchPositionIK: size of solution5

[DEBUG] [1373368035.033206859]: Solution passes callback

[ INFO] [1373368035.033500813]: IK_sol.size(): 0 ik_joint_bijection (move_group) size: 5

move_group: /home/sp/groovy_catkin_workspace/src/overlay/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp:568: bool constraint_samplers::IKConstraintSampler::callIK(const Pose&, const IKCallbackFn&, double, robot_state::JointStateGroup*, bool): Assertion `ik_sol.size() == ik_joint_bijection.size()' failed.

[move_group-1] process has died [pid 17149, exit code -6, cmd /home/sp/groovy_catkin_workspace/devel/lib/moveit_ros_move_group/move_group __name:=move_group __log:=/home/sp/.ros/log/9619509e-e885-11e2-8aec-e83935338487/move_group-1.log].

log file: /home/sp/.ros/log/9619509e-e885-11e2-8aec-e83935338487/move_group-1*.log

[Steffen P]

The solution for this problem has been merged into moveit_ikfast.

[Steffen P]

In RViz the ikfast plugin works perfectly now. I can move around the interactive marker and the missing degree of freedom is automatically calculated as expected.

But if I use e.g. the move_group.set_pose_target() and pass in a pose which is not reachable only because of the missing dof I get no solutions.

Shouldn't ikfast ignore the missing dof (in my case the yaw angle of the gripper) and return a valid solution?

Has anyone successfully used move_group.set_pose_target() with a 5 dof arm? (no matter if ikfast or KDL and without calculating the missing dof by hand)

[Adam Jardim]

I've been trying to do just that for a month or so now.  The most I'm able to do with my 5DOF is move to a position without orientation in KDL.  I do that by turning on "position_only_ik".  I'm still searching for a solution to this issue using ikfast, but my collada'd URDF never runs through ikfast, so that's my major issue.  I was hoping someone would chime in with good news on that.

[Steffen P]

Hey Ioan,

is there any progress on 5dof arm support for KDL?

I am still trying to get my 5 DOF Katana arm to work with moveit, currently with ikfast.

With a lot of hacking the moveit pick function works. But now I am stuck at the place function. 

The moveit place function takes the desired position of the attached object and calculates the needed gripper position from this information.

But this calculated gripper position is not reachable by the ik.

I have now commented out the PlacePlan::transformToEndEffectorGoal() function in place.cpp to be able to directly pass valid gripper poses. This works, but is not nice ;-)

I am still under Groovy but with the latest moveit.

Would it be better for me if I changed to Hydro? Are there changes concerning my issues?

Thanks for your help in advance!

-Steffen

2013/6/27 Ioan Sucan <isu...@willowgarage.com>

[Ioan Sucan]

Hello Steffen,

[others: read if you care about how pick & place gets input]

There have not been any fundamental changes in the pick& place pipeline in a while. I plan to take a look at that soon, but mostly to update the messages we use for specifying grasping poses.

The function you talk about handles the transform between the place location and grasping pose. The idea was that for pick you specify gripper poses and for place you specify object poses. We did this for legacy reasons (arm_navigation did it this way I believe). This said, I am open to removing that function, but its introduction caused a headache for some users, I'd rather not give them the same headache again. Anyone else have opinions on this?

Ioan