[ros-users] replicating pr2_arm_kinematics for 6-DOF arm

Rosen Diankov rosen.diankov at gmail.com
Sun Jul 18 22:48:15 UTC 2010

I forgot to mention that the files also contain an fk function for
each arm in pure C, so it should be ridiculously fast. The fk assumes
the base link is at the identity transformation, and gives the output
of the end effector link. You can use this for debugging and other

2010/7/18 Rosen Diankov <rosen.diankov at gmail.com>:
> The file generation might take ~5 min, so I'm attaching the cpp files
> in case you are having problems generating them.
> rosen,
> 2010/7/18 Rosen Diankov <rosen.diankov at gmail.com>:
>> Hi David,
>> Sorry about that, I should have explained the structure a little
>> better. Your urdf and collada file are fine. Thank you for pointing
>> out the problem with the documentation, I fixed it. You can generate
>> the fk file using this command:
>> openrave.py -p "open('fkfile.txt','w').write(env.GetBodies()[0].GetForwardKinematics());
>> sys.exit(0)" bandit.dae
>> However, it is probably simpler to create a small wrapper XML file
>> that specifies the manipulators that you'll be solving the IK for
>> (leftarm and rightarm in your case). We have plans to put this into
>> the urdf2collada parser eventually so we get complete integration, but
>> for now you have to create the xml file specifying the base and end
>> effector links (i'm attaching the one for your robot, please replace
>> the bandit.dae with your collada export).
>> Also, it looks like your 3 wrist axes do not intersect at a common
>> point, but your base 3 axes do. I just made some changes to openrave
>> to allow for this mechanism. Please do:
>> roscd openrave; svn up; rosmake openrave
>> Then using the attaching file, you would generate your ik with:
>> openrave.py --database inversekinematics --robot=bandit.robot.xml
>> --manipname=leftarm --freejoint=left_hand_thumb_joint
>> openrave.py --database inversekinematics --robot=bandit.robot.xml
>> --manipname=rightarm --freejoint=right_hand_thumb_joint
>> You should be able to compile them without a problem. The ik function
>> takes in the free parameter of the last joint on the wrist.
>> You can do sanity tests on it using:
>> openrave.py --database inversekinematics --robot=bandit.robot.xml
>> --manipname=leftarm --freejoint=left_hand_thumb_joint --usecached
>> --iktests=100
>> You can also do perf timing of the internal function:
>> openrave.py --database inversekinematics --robot=bandit.robot.xml
>> --manipname=leftarm --freejoint=left_hand_thumb_joint --usecached
>> --perftiming=1000
>> Note how the generated files are pretty big, this is because all
>> divide by 0 conditions and edge cases are being handled separately. In
>> some sense, even a human coder would have a hard time detecting all of
>> them.
>> rosen,
>> 2010/7/18 David Feil-Seifer <david.feilseifer at gmail.com>:
>>> Rosen-
>>> Thanks for the pointer. I had some trouble with urdf_to_collada and
>>> openrave when I tried it out. urdf_to_collada returns a dae file that
>>> I can view and it parses ok. However, when I view it in the openrave
>>> viewer it does no look right, the several of the joints are in the
>>> wrong place. I've attached the before and after to show you, though
>>> you'll need stl files to view it properly (several MBs).
>>> Further, when I tried to use openrave.py --database, I get several of
>>> the following errors:
>>>> rosrun openrave inversekinematics.py --robot=bandit_current.dae --manipname=rightarm --freejoint=right_torso_shoulder_mounting_joint --freeinc=0.01
>>> [colladareader.h:1613] Not Exists Motion axis info, joint
>>> right_torso_shoulder_mounting_joint
>>> Terminating with:
>>> [colladareader.h:1176] Robot (null) created ...
>>> destroying environment
>>> Traceback (most recent call last):
>>>  File "/home/dfseifer/cturtle_usc/stacks/openrave_planning/openrave/share/openrave/openravepy/databases/inversekinematics.py",
>>> line 430, in <module>
>>>    InverseKinematicsModel.RunFromParser()
>>>  File "/home/dfseifer/cturtle_usc/stacks/openrave_planning/openrave/share/openrave/openravepy/databases/inversekinematics.py",
>>> line 402, in RunFromParser
>>>    OpenRAVEModel.RunFromParser(Model=Model,parser=parser)
>>>  File "/home/dfseifer/cturtle_usc/stacks/openrave_planning/openrave/share/openrave/openravepy/openravepy_ext.py",
>>> line 469, in RunFromParser
>>>    robot.SetActiveManipulator([i for i,m in
>>> enumerate(robot.GetManipulators()) if
>>> m.GetName()==options.manipname][0])
>>> IndexError: list index out of range
>>> I'm guessing that the combination of the two errors means that
>>> something is up with my urdf file that is getting botched when it goes
>>> to collada, but I don't know the format well enough to view it by
>>> inspection.
>>> For ikfast.py, I can wait for the documentation, but while I
>>> understand the command-line options from the --help menu, I do not
>>> know how to get the fkfile. Does this come from openrave.py
>>> --database?
>>> Anyway, thanks for your help. I'm not in a rush on this, but I am
>>> curious how to get this working.
>>> -Dave
>>> On Sat, Jul 17, 2010 at 5:46 PM, Rosen Diankov <rosen.diankov at gmail.com> wrote:
>>>> I am in the process of writing better documentation, but if you have
>>>> your robot in COLLADA or OpenRAVE XML format, then you can use the
>>>> ikfast program to generate the analytical inverse kinematics:
>>>> http://openrave.programmingvision.com/index.php?title=Component:Ikfast
>>>> http://openrave.programmingvision.com/index.php/Databases:InverseKinematics
>>>> from all the timings i've done, these ikfast solvers can generate all
>>>> possible solutions for you in ~6 microseconds.
>>>> rosen,
>>>> 2010/7/17 Morgan Quigley <mquigley at cs.stanford.edu>:
>>>>> Hi Dave,
>>>>> The Orocos KDL library is great. I just switched to it last week; my
>>>>> code is pretty ugly, but it's using the KDL::ChainIkSolverPos_NR_JL
>>>>> class, which has joint limits:
>>>>> http://stanford-ros-pkg.googlecode.com/svn/trunk/openarms/control/goto_cart.cpp
>>>>> The ik_tool() function is what you're after. You set the joint limits
>>>>> when you construct the ChainIkSolverPos_NR_JL object; search for
>>>>> "ik_solver_pos" in that .cpp file to see the exact syntax.
>>>>> (sorry about all the commented code... it's a work in progress)
>>>>> HTH,
>>>>> Morgan
>>>>> On Sat, Jul 17, 2010 at 10:43 AM, David Feil-Seifer
>>>>> <david.feilseifer at gmail.com> wrote:
>>>>>> I am trying to replicate the IK-functionality of pr2_arm_navigation
>>>>>> for our humanoid robot, Bandit. I have attached a xacro file for it's
>>>>>> arm. I have it rendering correctly, and am able to have it mimic the
>>>>>> movements of a real Bandit with joint_states_publisher and a
>>>>>> /joint_states message. I seem to have correctly ported
>>>>>> pr2_arm_ik_solver and pr2_arm_kinematics by simply changing all 7's to
>>>>>> 6's and changing the names of the root and tip links
>>>>>> (bandit_torso_link and left/right_hand_link). However, I'm having
>>>>>> trouble with pr2_arm_ik.cpp. Clearly, the math here was meant for a
>>>>>> 7-DOF arm, and I don't really know what algorithm is being used here,
>>>>>> so I'm having trouble translating that to a 6-DOF arm. I have figured
>>>>>> out that in the init function, that I needed to change the
>>>>>> link_offsets for the shoulder_upperarm_offset, elbow_wrist offset, and
>>>>>> upperarm_elbow_offset values. Those are now correct. I am just having
>>>>>> trouble making computeIKShoulderPan and computeIKShoulderRoll to a 6
>>>>>> joint arm. Can anyone give me some guidance on how to proceed?
>>>>>> I did look at the KDL examples on their website, but they were pretty
>>>>>> vague about creating an IK solver with joint limits.
>>>>>> Thanks for your help.
>>>>>> -Dave
>>>>>> _______________________________________________
>>>>>> ros-users mailing list
>>>>>> ros-users at code.ros.org
>>>>>> https://code.ros.org/mailman/listinfo/ros-users
>>>>> _______________________________________________
>>>>> ros-users mailing list
>>>>> ros-users at code.ros.org
>>>>> https://code.ros.org/mailman/listinfo/ros-users
>>>> _______________________________________________
>>>> ros-users mailing list
>>>> ros-users at code.ros.org
>>>> https://code.ros.org/mailman/listinfo/ros-users
>>> _______________________________________________
>>> ros-users mailing list
>>> ros-users at code.ros.org
>>> https://code.ros.org/mailman/listinfo/ros-users

More information about the ros-users mailing list