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 : > 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 : >> 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 >>    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 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 : >>>> 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 >>>> 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@code.ros.org >>>>> https://code.ros.org/mailman/listinfo/ros-users >>>>> >>>>> >>>> _______________________________________________ >>>> ros-users mailing list >>>> ros-users@code.ros.org >>>> https://code.ros.org/mailman/listinfo/ros-users >>>> >>> _______________________________________________ >>> ros-users mailing list >>> ros-users@code.ros.org >>> https://code.ros.org/mailman/listinfo/ros-users >>> >> >> _______________________________________________ >> ros-users mailing list >> ros-users@code.ros.org >> https://code.ros.org/mailman/listinfo/ros-users >> >> >