0
$\begingroup$

Rosanswers logo

OS: Ubuntu 18.04 LTS

ROS Distro: ROS Melodic

Package: MoveIt

Planner: OMPL

IK Solver: KDL Kinematics Plugin

After working with panda arm in MoveIt, I wanted to work on my own designed 6 DOF arm.

I have attached an image with the model in Rviz.

image description

When working with Move Group Interface, I am able to give joint space goals. But when working with pose goals, the kinematics solver is not able to solve any pose goals provided. To be very specific, it is only able to solve configurations where position.x = 0 and rotation is also only about x-axis.

Also, when I set the start and goal pose using interactive markers in rviz, it is able to plan and execute the goals. I don't really know whether internally it is using pose goal or joint space goal.

After analysing the design difference between my model and the panda arm model, I feel that most of the joints in my model are biased to rotations about x-axis which could be a potential issue given that poses with position.x = 0 are working.

So my question is, is the issue really due to design flaw or is there something I am missing?

EDIT:

Terminal Output of demo.launch:

[ INFO] [1621137166.334511082]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1621137166.504758641]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1621137166.556362808]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1621137171.561261517]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1621137171.561412783]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1621137171.561477077]: No solution found after 5.005472 seconds
[ INFO] [1621137171.580672288]: Unable to solve the planning problem
[ INFO] [1621137171.634753681]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1621137171.635195443]: Planning attempt 1 of at most 1
[ INFO] [1621137171.636336136]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1621137171.636810730]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1621137176.640755824]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1621137176.640902036]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1621137176.640945267]: No solution found after 5.004324 seconds
[ INFO] [1621137176.659373442]: Unable to solve the planning problem

Terminal Output of My Code:

[ INFO] [1621137153.347472082]: Loading robot model 'robot'...
[ INFO] [1621137154.701934459]: Ready to take commands for planning group arm.
[ INFO] [1621137158.041919817]: RemoteControl Ready.
[ INFO] [1621137158.308677700]: Planning frame: base_link
[ INFO] [1621137158.308766541]: End effector link: gripper_attacher
[ INFO] [1621137158.308833987]: Available Planning Groups: 
arm, hand, 
Waiting to continue: Press 'next', in RvizVisualGui window to start the demo... continuing
[ WARN] [1621137171.581395991]: Fail: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1621137171.581491845]: Visualizing plan 1 (pose goal FAILED)
[ INFO] [1621137176.660045669]: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1621137176.660168757]: Visualizing plan 1 as a trajectory line
[ INFO] [1621137176.660337712]: No planning scene passed into moveit_visual_tools, creating one.
[ INFO] [1621137176.671130270]: Loading robot model 'robot'...
[ INFO] [1621137176.862872864]: Publishing maintained planning scene on ''
[ WARN] [1621137177.865424904]: Skipping path because 0 points passed in.

URDF:

<?xml version="1.0"?>
<robot name="robot" xmlns:xacro="http://ros.org/wiki/xacro">
    <!-- Constants -->
    <xacro:property name="pi" value="3.14159265"/>

    <!-- Model Parameters -->


    <!-- VISUAL PROPS -->
    <material name="red">
        <color rgba="1 0 0 1" />
    </material>
    <material name="green">
        <color rgba="0 1 0 1" />
    </material>
    <material name="blue">
        <color rgba="0 0 1 1" />
    </material>
    <material name="yellow">
        <color rgba="1 1 0 1" />
    </material>
    <material name="black">
        <color rgba="0 0 0 1" />
    </material>
    <material name="white">
        <color rgba="1 1 1 1" />
    </material>

    <!-- INERTIAL FUNCTIONS -->
    <xacro:macro name="default_inertial" params="mass">
        <inertial>
            <mass value="${mass}" />
            <inertia 
            ixx="1.0"   ixy="0.0"   ixz="0.0" 
                        iyy="1.0"   iyz="0.0" 
                                    izz="1.0"
            />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cuboid_inertial" params="mass l w h">
        <inertial>
            <mass value="${mass}" />
            <inertia 
            ixx="${1/12*mass*(h*h + l*l)}"   ixy="0.0"   ixz="0.0" 
                        iyy="${1/12*mass*(w*w + l*l)}"   iyz="0.0" 
                                    izz="${1/12*mass*(w*w + h*h)}"
            />
        </inertial>
    </xacro:macro>
    
    <xacro:macro name="cylinder_inertial" params="mass r h">
        <inertial>
            <mass value="${mass}" />
            <inertia 
            ixx="${1/12*mass*(3*r*r + h*h)}"   ixy="0.0"   ixz="0.0" 
                        iyy="${1/12*mass*(3*r*r + h*h)}"   iyz="0.0" 
                                    izz="${1/2*mass*(r*r)}"
            />
        </inertial>
    </xacro:macro>

    <!-- MODEL -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.15 0.15 0.005"/>                
            </geometry>
            
            <material name="black"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.15 0.15 0.005"/>                
            </geometry>
        </collision>
    </link>

    <joint name="base_rotate_joint" type="revolute">
        <parent link="base_link"/>
        <child link="base"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="0.7"/>
    </joint>
    <link name="base">
        <visual>
            <origin xyz="0 0 0.0525"/>
            <geometry>
                <cylinder radius="0.06" length="0.1"/>                
            </geometry>

            <material name="black"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.0525"/>
            <geometry>
                <cylinder radius="0.06" length="0.1"/>                
            </geometry>
        </collision>
    </link>

    <joint name="shoulder_pitch_joint" type="revolute">
        <parent link="base"/>
        <child link="shoulder"/>
        <origin xyz="0 0 0.1" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
        <limit effort="1000.0" lower="${-pi/2}" upper="${pi/2}" velocity="0.7"/>
    </joint>
    <link name="shoulder">
        <visual>
            <origin xyz="0 0 0.135"/>
            <geometry>
                <box size="0.03 0.03 0.3"/>                
            </geometry>

            <material name="yellow"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.135"/>
            <geometry>
                <box size="0.03 0.03 0.3"/>                
            </geometry>
        </collision>
    </link>

    <joint name="elbow_support_joint" type="fixed">
        <parent link="shoulder"/>
        <child link="elbow_support"/>
        <origin xyz="0 0 0.285" rpy="0 0 0"/>
    </joint>
    <link name="elbow_support">
        <visual>
            <origin xyz="0 0 0.035"/>
            <geometry>
                <cylinder radius="0.03" length="0.1"/>                
            </geometry>

            <material name="black"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.035"/>
            <geometry>
                <cylinder radius="0.03" length="0.1"/>                
            </geometry>
        </collision>
    </link>

    <joint name="elbow_pitch_joint" type="revolute">
        <parent link="elbow_support"/>
        <child link="elbow"/>
        <origin xyz="0 0 0.085" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
        <limit effort="1000.0" lower="${-3*pi/4}" upper="${3*pi/4}" velocity="0.7"/>
    </joint>
    <link name="elbow">
        <visual>
            <origin xyz="0 0 0.135"/>
            <geometry>
                <box size="0.03 0.03 0.3"/>                
            </geometry>

            <material name="yellow"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.135"/>
            <geometry>
                <box size="0.03 0.03 0.3"/>                
            </geometry>
        </collision>
    </link>

    <joint name="wrist_support_joint" type="fixed">
        <parent link="elbow"/>
        <child link="wrist_support"/>
        <origin xyz="0 0 0.285" rpy="0 0 0"/>
    </joint>
    <link name="wrist_support">
        <visual>
            <origin xyz="0 0 0.035"/>
            <geometry>
                <cylinder radius="0.03" length="0.1"/>                
            </geometry>

            <material name="black"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.035"/>
            <geometry>
                <cylinder radius="0.03" length="0.1"/>                
            </geometry>
        </collision>
    </link>

    <joint name="wrist_pitch_joint" type="revolute">
        <parent link="wrist_support"/>
        <child link="wrist_pitch"/>
        <origin xyz="0 0 0.085" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
        <limit effort="1000.0" lower="${-pi/2}" upper="${pi/2}" velocity="0.7"/>
    </joint>
    <link name="wrist_pitch">
        <visual>
            <origin xyz="0 0 0.03"/>
            <geometry>
                <box size="0.03 0.03 0.09"/>                
            </geometry>

            <material name="yellow"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.03"/>
            <geometry>
                <box size="0.03 0.03 0.09"/>                
            </geometry>
        </collision>
    </link>

    <joint name="eef_support_joint" type="fixed">
        <parent link="wrist_pitch"/>
        <child link="eef_support"/>
        <origin xyz="0 0 0.075" rpy="0 0 0"/>
    </joint>
    <link name="eef_support">
        <visual>
            <origin xyz="0 0 0.035"/>
            <geometry>
                <cylinder radius="0.03" length="0.1"/>                
            </geometry>

            <material name="black"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.035"/>
            <geometry>
                <cylinder radius="0.03" length="0.1"/>                
            </geometry>
        </collision>
    </link>

    <joint name="eef_roll_joint" type="revolute">
        <parent link="eef_support"/>
        <child link="eef_roll"/>
        <origin xyz="0 0 0.085" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="0.7"/>
    </joint>
    <link name="eef_roll">
        <visual>
            <origin xyz="0 0 0.0025"/>
            <geometry>
                <cylinder radius="0.05" length="0.005"/>                
            </geometry>

            <material name="yellow"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.0025"/>
            <geometry>
                <cylinder radius="0.05" length="0.005"/>                
            </geometry>
        </collision>
    </link>
    
    <joint name="eef_pitch_joint" type="revolute">
        <parent link="eef_roll"/>
        <child link="eef_pitch"/>
        <origin xyz="0 0 0.025" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
        <limit effort="1000.0" lower="${-pi/4}" upper="${pi/4}" velocity="0.7"/>
    </joint>
    <link name="eef_pitch">
        <visual>
            <origin xyz="0 0 0"  rpy="0 ${pi/2} 0"/>
            <geometry>
                <cylinder radius="0.02" length="0.08"/>                
            </geometry>

            <material name="black"/>
        </visual>

        <collision>
            <origin xyz="0 0 0" rpy="0 ${pi/2} 0"/>
            <geometry>
                <cylinder radius="0.02" length="0.08"/>                
            </geometry>
        </collision>
    </link>

    <joint name="gripper_attacher_joint" type="fixed">
        <parent link="eef_pitch"/>
        <child link="gripper_attacher"/>
        <origin xyz="0 0 0.02" rpy="0 0 0"/>
    </joint>
    <link name="gripper_attacher">
        <visual>
            <origin xyz="0 0 0.005"/>
            <geometry>
                <box size="0.1 0.075 0.01"/>                
            </geometry>

            <material name="white"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.005"/>
            <geometry>
                <box size="0.1 0.075 0.01"/>                
            </geometry>
        </collision>
    </link>
    <joint name="gripper_support_joint" type="fixed">
        <parent link="gripper_attacher"/>
        <child link="gripper_support"/>
        <origin xyz="0 0 0.01" rpy="0 0 0"/>
    </joint>
    <link name="gripper_support">
        <visual>
            <origin xyz="0 0 0.005"/>
            <geometry>
                <box size="0.1 0.04 0.01"/>                
            </geometry>

            <material name="green"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.005"/>
            <geometry>
                <box size="0.1 0.04 0.01"/>                
            </geometry>
        </collision>
    </link>

    <joint name="gripper_leftfinger_joint" type="prismatic">
        <parent link="gripper_support"/>
        <child link="gripper_leftfinger"/>
        <origin xyz="-0.03 0 0.01" rpy="0 0 ${pi/2}"/>
        <axis xyz="0 1 0"/>
        <limit effort="1000.0" lower="0" upper="-0.02" velocity="0.7"/>
    </joint>
    <link name="gripper_leftfinger">
        <visual>
            <origin xyz="0 0 0.04"/>
            <geometry>
                <box size="0.02 0.02 0.1"/>                
            </geometry>

            <material name="blue"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.04"/>
            <geometry>
                <box size="0.02 0.02 0.1"/>                
            </geometry>
        </collision>
    </link>

    <joint name="gripper_rightfinger_joint" type="prismatic">
        <parent link="gripper_support"/>
        <child link="gripper_rightfinger"/>
        <origin xyz="0.03 0 0.01" rpy="0 0 ${-pi/2}"/>
        <axis xyz="0 1 0"/>
        <limit effort="1000.0" lower="0" upper="-0.02" velocity="0.7"/>
    </joint>
    <link name="gripper_rightfinger">
        <visual>
            <origin xyz="0 0 0.04"/>
            <geometry>
                <box size="0.02 0.02 0.1"/>                
            </geometry>

            <material name="blue"/>
        </visual>

        <collision>
            <origin xyz="0 0 0.04"/>
            <geometry>
                <box size="0.02 0.02 0.1"/>                
            </geometry>
        </collision>
    </link>
</robot>

Originally posted by omkar_a_k on ROS Answers with karma: 25 on 2021-05-15

Post score: 0


Original comments

Comment by fvd on 2021-05-15:
Please post your URDF and the errors from your IK solver

Comment by fvd on 2021-05-16:
If you say you can drag the interactive marker and find joint configurations in the RViz window, it sounds like your IK plugin working. Are you sure it's not an incorrect orientation or choice of endEffectorLink? Is the planning group defined properly for your 6 robot arm joints? It looks like your group contains all of them.

Comment by omkar_a_k on 2021-05-16:
I used current command from moveit_commander_cmdline.py to grab the end effector pose. And it kind of worked. I had tried that even before posting the question but maybe I missed something that time. So I tried it again today and that worked. Then I realised that the end effector prefers a certain range of orientations to cover maximum space. But even that space is quite restricted. I also did my homework yesterday by adding more degrees of freedom. I removed eef_pitch_joint and added elbow_roll_joint and wrist_roll_joint making it 7 DOF. This model is working perfectly for most pose goals. I guess then the pose space is quite restricted due the design and that was the problem after all.

Comment by fvd on 2021-05-16:
I see, that's good. Please post the explanation as a response and accept it to mark this problem as solved.

Comment by fvd on 2021-05-16:
Come to think of it, I didn't think to check if your 5 DOFs are independent (e.g. no two subsequent joints rotating in the same plane). That would have been another consideration.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I used current command from moveit_commander_cmdline.py to grab the end effector pose. And it kind of worked. Then I realised that the end effector prefers a certain range of orientations to cover maximum space. But even that space is quite restricted. I also tried by adding more degrees of freedom. I removed eef_pitch_joint and added elbow_roll_joint and wrist_roll_joint making it 7 DOF. This model is working perfectly for most pose goals. I guess then the pose space is quite restricted due the design and that was the problem after all.


Originally posted by omkar_a_k with karma: 25 on 2021-05-16

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.