ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

moveit: cannot move interactive marker with IKfast

asked 2016-03-29 17:39:04 -0500

PizzaTime gravatar image

Hello
i´ve a problem with ikfast for moveit. i´ve gone trough all tutorials and KDL and Trac_ik are working fine in rviz. However when i´m using the 3DOF IKfast i cant set the goal position with the marker or interact in any way with the leg with Move Group Interface. The leg is a simple leg with 3 actors in a row.

when i print openrave-robot.py <myrobot_name>.dae --info links i´ve got the following links

base_link     0                  
thorax        1     base_link    
leg_center_l1 2     thorax       
coxa_l1       3     leg_center_l1
femur_l1      4     coxa_l1      
tibia_l1      5     femur_l1     
tibia_foot_l1 6     tibia_l1

the ik-fast was calclulated with

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=<myrobot_name>.dae --iktype=translation3d --baselink=2 --eelink=6 --savefile=ik_leg1.cpp

so it is the exact same problem as in this case https://www.youtube.com/watch?v=M1c8A... but using "position_only_ik: true" in kinematics.yaml doesnt solve the problem

leg_1:
  kinematics_solver: group_kinematics/IKFastKinematicsPlugin
  kinematics_solver_attempts: 3
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  position_only_ik: true

I assume that the IK cannot solve for the solution and somehow the srdf may be wrong. I´ve used many configurations in the srdf from assigning only joints and one eef.

    <group name="leg_1">
            <joint name="base_joint" />
            <joint name="leg_center_joint_l1" />
            <joint name="coxa_joint_l1" />
            <joint name="femur_joint_l1" />
            <joint name="tibia_joint_l1" />
            <joint name="tibia_foot_joint_l1" />
        </group>
  <end_effector name="leg_1_foot" parent_link="tibia_foot_l1" group="leg_1" />

as putting putting everythin in a chain, put the rest in a subgroup

    <group name="leg_1">
        <chain base_link="thorax" tip_link="tibia_l1" />
    </group>
    <group name="leg_test">
        <joint name="leg_center_joint_l1" />
        <joint name="coxa_joint_l1" />
        <joint name="femur_joint_l1" />
        <joint name="tibia_joint_l1" />
        <joint name="tibia_foot_joint_l1" />
        <joint name="base_joint" />
    </group>
<end_effector name="leg1eef" parent_link="tibia_l1" group="leg_test" parent_group="leg_1" />
edit retag flag offensive close merge delete

Comments

Have you ticked the Approximate IK checkbox in the RViz MoveIt plugin UI? I'm not sure just setting position_only_ik: true is enough for the RViz plugin. It does influence the programmatic interface, but the interactive marker may be a separate thing.

gvdhoorn gravatar image gvdhoorn  ( 2016-03-30 10:36:19 -0500 )edit

Hello, Yes i´ve tried to allow approximate IK and also set collisions to ignore.

PizzaTime gravatar image PizzaTime  ( 2016-03-30 11:27:43 -0500 )edit

i´ve tried to use a newer openrave version without any success. do you know if there is a convenient way to test the generated ik?

PizzaTime gravatar image PizzaTime  ( 2016-03-30 12:53:54 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2016-03-30 14:08:59 -0500

PizzaTime gravatar image

updated 2016-03-30 16:42:47 -0500

Ok so now it runs roughly and very rugged what have i done?

  1. I've installed the recommended version of sympy as told in this wiki http://wiki.ros.org/Industrial/Tutori...

  2. i've simplified the dae and used only the arm i want to calculate

  3. i've rounded the dae

    rosrun moveit_ikfast round_collada_numbers.py <myrobot_name>.dae <myrobot_name>.rounded.dae 5

  4. regenereated the ikfast solution... PS: without the correct sympy version is was impossible to generate the ikfast for the rounded.dae

but still it is not usable and i'm still unable to use the move_group_interface to set any valid positions


ok i got it :-) problem was finally solved when i changed my urdf model and removed all stl files. the generated ik_fast solver was significant smaller and now everything works perfekt!

edit flag offensive delete link more

Comments

Thank you for your answer, it give me some guidance. But, there are still some qusetions. I built my robot by SolidWorks, and I export it as urdf. The robot description are some links to the mesh.STL in the urdf. I meet the same problem as you. Should I transform mesh.STL to mesh.dae , as your answer? If I should, what I should watch out in this process?

nuboter_cc gravatar image nuboter_cc  ( 2020-08-22 22:37:01 -0500 )edit

Question Tools

Stats

Asked: 2016-03-29 17:39:04 -0500

Seen: 970 times

Last updated: Mar 30 '16