[ros-users] replicating pr2_arm_kinematics for 6-DOF arm
Ugo Cupcic
ugo at shadowrobot.com
Fri Aug 13 08:39:18 UTC 2010
Hi,
I'm trying to get reverse kinematics working for our 4DOF arm. I based
my code on goto_cart.cpp, but I'm having problems getting it to work.
My program seg fault when calling the ik_solver->CartToJnt(q_init,
destination_frame, q).
Any idea?
The backlog is below.
Cheers,
Ugo
Program received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0xb46ffb70 (LWP 4670)]
Ogre::processManualProgramParam (isNamed=232, commandname=...,
vecparams=...,
context=..., index=225, paramName=...)
at
/opt/ros/cturtle/stacks/visualization_common/ogre/build/ogre_src_v1-7-1/OgreMain/src/OgreMaterialSerializer.cpp:1944
1944 realBuffer[i] = 0.0f;
(gdb) backtrace
#0 Ogre::processManualProgramParam (isNamed=232, commandname=...,
vecparams=..., context=..., index=225, paramName=...)
at
/opt/ros/cturtle/stacks/visualization_common/ogre/build/ogre_src_v1-7-1/OgreMain/src/OgreMaterialSerializer.cpp:1944
#1 0x08087d15 in shadowhand::SrKinematics::computeReverseKinematics (
this=0x812d438, transform=..., joints=...)
at
/home/ugo/Projects/ROS_interfaces/sr-ros-interface/reverse-kinematic/shadow_robot/sr_hand/src/sr_kinematics.cpp:226
#2 0x0806d361 in
shadowhand_subscriber::ShadowhandSubscriber::reverseKinematicsCallback
(this=0x8136510, msg=...)
at
/home/ugo/Projects/ROS_interfaces/sr-ros-interface/reverse-kinematic/shadow_robot/sr_hand/src/sr_subscriber.cpp:175
#3 0x08077105 in
boost::detail::function::void_function_obj_invoker1<boost::function<void
()(boost::shared_ptr<tf::tfMessage_<std::allocator<void> > const>
const&)>, void, boost::shared_ptr<tf::tfMessage_<std::allocator<void> >
const> >::invoke(boost::detail::function::function_buffer&,
boost::shared_ptr<tf::tfMessage_<std::allocator<void> > const>) ()
#4 0x080799d0 in
ros::SubscriptionCallbackHelperT<boost::shared_ptr<tf::tfMessage_<std::allocator<void>
> const> const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) ()
#5 0x01441a4d in ros::SubscriptionQueue::call (this=0xb4704020)
at
/opt/ros/cturtle/ros/core/roscpp/src/libros/subscription_queue.cpp:164
#6 0x01485aea in ros::CallbackQueue::callOneCB (this=0x811b398,
tls=0x813b170)
---Type <return> to continue, or q <return> to quit---
at /opt/ros/cturtle/ros/core/roscpp/src/libros/callback_queue.cpp:381
#7 0x014861df in ros::CallbackQueue::callAvailable (this=0x811b398,
timeout=...)
at /opt/ros/cturtle/ros/core/roscpp/src/libros/callback_queue.cpp:333
#8 0x01412a2b in ros::spinOnce ()
at /opt/ros/cturtle/ros/core/roscpp/src/libros/init.cpp:498
#9 0x08083485 in shadowhand_diagnosticer::ShadowhandDiagnosticer::publish (
this=0x8128020)
at
/home/ugo/Projects/ROS_interfaces/sr-ros-interface/reverse-kinematic/shadow_robot/sr_hand/src/sr_diagnosticer.cpp:576
#10 0x0807f07e in run_diagnotics (shadowhand_diag=DWARF-2 expression
error: DW_OP_reg operations must be used either alone or in conjuction
with DW_OP_piece.
)
at
/home/ugo/Projects/ROS_interfaces/sr-ros-interface/reverse-kinematic/shadow_robot/sr_hand/src/hand/virtual_arm_node.cpp:43
#11 0x08081843 in boost::detail::thread_data<boost::_bi::bind_t<void,
void
(*)(boost::shared_ptr<shadowhand_diagnosticer::ShadowhandDiagnosticer>),
boost::_bi::list1<boost::_bi::value<boost::shared_ptr<shadowhand_diagnosticer::ShadowhandDiagnosticer>
> > > >::run() ()
#12 0x015b37c5 in thread_proxy () from /usr/lib/libboost_thread.so.1.40.0
#13 0x009d596e in start_thread () from /lib/tls/i686/cmov/libpthread.so.0
#14 0x019b7a4e in clone () from /lib/tls/i686/cmov/libc.so.6
On 23/07/10 20:19, David Feil-Seifer wrote:
> OK, fixed. It turned out that the problem was in the way goto_cart.cpp
> was translating the point into the torso's frame. The attached version
> fixes the problem.
>
> On Tue, Jul 20, 2010 at 7:18 PM, Morgan Quigley
> <mquigley at cs.stanford.edu> wrote:
>> Hi Dave,
>>
>> I'm definitely not the right person to ask about KDL, but perhaps the
>> experts lurking on this list can help you out. However, in general
>> with IK systems when this happens, you might try to loosen the
>> tolerance on the solution, and (more importantly) when debugging
>> things, give it your exact current joint configuration as the IK
>> starting point, and the exact forward-kinematic position as your
>> target position. That way you know you aren't asking the solver to go
>> through an infeasible region on its way to the solution(s). If that
>> fails, then I would suspect the error is in your URDF, and that's
>> where urdf_to_graphviz and rviz can help out:
>>
>> http://www.ros.org/wiki/urdf/Tutorials/Create%20your%20own%20urdf%20file
>>
>> HTH,
>> Morgan
>>
>> p.s., also triple-check each argument you're passing into KDL, such as
>> the length of all the joint vectors, etc. It's easy to make a typo and
>> end up with a generic error message like this.
>>
>>
>>
>> On Tue, Jul 20, 2010 at 5:03 PM, David Feil-Seifer
>> <david.feilseifer at gmail.com> wrote:
>>> I finally gave this a try and it did not work for me. Here is a
>>> rewritten version to get joint bound info from the URDF file and to
>>> get joint names from the urdf description as well. I've attached a
>>> revised version.
>>>
>>> I tried this on both the PR2 and the Bandit model, and I kept getting
>>> and "ik solver fail" error. This would occur even if I gave the
>>> robot's current end-effector position. Attached are two files, on is a
>>> revised goto_cart.cpp and another is a file that sends a test message.
>>> To run on the pr2 model in gazebo:
>>>
>>> ./goto_cart _root_name:="l_shoulder_pan_link" _tip_name:="l_wrist_roll_link"
>>> ./cmd_ik
>>>
>>> Are you using a version of kdl newer than what is being used in
>>> cturtle? Have I messed something up with updating your file?
>>>
>>> On Sat, Jul 17, 2010 at 11:37 AM, Morgan Quigley
>>> <mquigley at cs.stanford.edu> wrote:
>>>> 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
>>
>>
>>
>> _______________________________________________
>> ros-users mailing list
>> ros-users at code.ros.org
>> https://code.ros.org/mailman/listinfo/ros-users
--
Ugo Cupcic | Shadow Robot Company | ugo at shadowrobot.com
Software Engineer | 251 Liverpool Road |
need a Hand? | London N1 1LX | +44 20 7700 2487
http://www.shadowrobot.com/hand/ @shadowrobot
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.osuosl.org/pipermail/ros-users/attachments/20100813/8d747906/attachment-0002.html>
More information about the ros-users
mailing list