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

Sun Jul 18 22:42:35 UTC 2010

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
>
>
-------------- next part --------------
A non-text attachment was scrubbed...
Name: bandit.robot.xml
Type: text/xml
Size: 285 bytes
Desc: not available
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20100718/752f9c48/attachment-0003.xml>