0
$\begingroup$

Rosanswers logo

Hi,

I have a UR5 robotic arm. The arm is mounted on a table surface. I have created the following xacro (workcell.xacro) for a combination of ur5.urdf.xacro and a table link.

<?xml version="1.0" ?>
<robot name="myworkcell" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
<xacro:ur5_robot prefix="" joint_limited="true"/>

<link name="world"/>
<link name="table">
  <visual>
    <geometry>
      <box size="1.0 1.0 0.05"/>
    </geometry>
  </visual>
  <collision>
    <geometry>
      <box size="1.0 1.0 0.05"/>
    </geometry>
  </collision>
</link>

<joint name="world_to_table" type="fixed">
  <parent link="world"/>
  <child link="table"/>
  <origin xyz="0 0 0.5" rpy="0 0 0"/>
</joint>

<joint name="table_to_robot" type="fixed">
  <parent link="table"/>
  <child link="base_link"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>


</robot>

I used the Moveit Setup Assistant (MSA) to generate a moveit configuration package based on my workcell.xacro, and the screen shot depicts as follows: image description

For the planning group manipulator I have specified the following kinetic chain (base_link -> tool0). The srdf myworkcell.srdf is given as follows:

<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="myworkcell">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="manipulator">
        <chain base_link="base_link" tip_link="tool0" />
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="home" group="manipulator">
        <joint name="elbow_joint" value="1.5707" />
        <joint name="shoulder_lift_joint" value="-1.5707" />
        <joint name="shoulder_pan_joint" value="0" />
        <joint name="wrist_1_joint" value="-1.5707" />
        <joint name="wrist_2_joint" value="-1.5707" />
        <joint name="wrist_3_joint" value="0" />
    </group_state>
    <group_state name="home_2" group="manipulator">
        <joint name="elbow_joint" value="1.5707" />
        <joint name="shoulder_lift_joint" value="-1.5707" />
        <joint name="shoulder_pan_joint" value="1.5707" />
        <joint name="wrist_1_joint" value="-1.5707" />
        <joint name="wrist_2_joint" value="-1.5707" />
        <joint name="wrist_3_joint" value="0" />
    </group_state>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="world" />
    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
    <disable_collisions link1="base_link" link2="table" reason="Adjacent" />
    <disable_collisions link1="ee_link" link2="wrist_1_link" reason="Never" />
    <disable_collisions link1="ee_link" link2="wrist_2_link" reason="Never" />
    <disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent" />
    <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
    <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
    <disable_collisions link1="shoulder_link" link2="table" reason="Default" />
    <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
    <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
    <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
    <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
</robot>

With these setting, I have created a moveit_planning_execution.launch file in which I include necessary nodes, as well as, driver launch files. This is the sequence of commands I run.

roslaunch cch_moveit_pkg moveit_planning_execution.launch sim:=false robot_ip:=192.168.0.103
rosrun cch_moveit_pkg pure_motion_node 1

On the RViz window, and adding the MotionPlanning plugin, I can successfully plan & execute motions to different random valid goal positions. However, the goal pose that I send through my code, never gets successful. The goal pose is given by the following string:

std::string s = "[-0.14527758743934377, 0.5449976892506504, 0.38858309510149047, -0.15787628477528537, 0.2747166458598104, 0.5450830459979051]";

[ INFO] [1645613387.124152053]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1645613397.384834881]: Found a contact between 'table' (type 'Robot link') and 'upper_arm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
...
[ERROR] [1645613407.123761592]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1645613407.123798563]: RRTConnect: Created 1 states (1 start + 0 goal)
[ERROR] [1645613407.132164341]: RRTConnect: Unable to sample any valid states for goal tree

The position of the goal is the one that I read on Teach Pendant, And, it is in the free space. Does ROS expect the goal position in a different frame than with respect to the base. What is the cause of this error.

Prior to this, when I did not model table in the urdf, the arm could move to the goal position, and it was correctly reached (with only small deviations in the final position). There was always an error with respect to orientation. But, for now, my main concern is having the robotic arm move to the goal position.

thanks,

Zahid

Originally posted by zahid990170 on ROS Answers with karma: 135 on 2022-02-23

Post score: 0

Original comments

Comment by gvdhoorn on 2022-02-23:
Did you consider the origin of a box geometry is in the centre, not at any of the surfaces making up the box?

The way you've structured the .xacro has the base_link of the UR inside the table. That's probably not what you wanted to do.

Comment by zahid990170 on 2022-02-23:
thanks @gvdhoorn. The origin tag is confusing to understand. I have modified the xacro file as follows.

<joint name="table_to_robot" type="fixed">
  <parent link="table"/>
  <child link="base_link"/>
  <origin xyz="0 0 0.025" rpy="0 0 0"/>
</joint>

I will see if that can sort out the problem.

Comment by zahid990170 on 2022-02-23:
I have some further questions, If I issue two calls, setPositionTarget(), setOrientationTarget(), I can see the movements being done on the robotic arm. However, the above two functions expect end-effector link to be provided in the last argument. In the MSA I did not choose any end-effectors, and modeled a single planning group (manipulator / arm) that includes the complete chain (base_link -> tool0). In this case, would that effect the result of the movement. In the function call, I am passing setPositionTarget(x, y, z, move_group.getEndEffectorLink()). thanks,

Comment by Mike Scheutzow on 2022-03-02:
You closed this question saying it was answered, but you have not shared what that answer is. If you can, please provide an answer to your own question for future readers.

Comment by zahid990170 on 2022-03-03:
unfortunately, I couldn't reopen the question. Its just some unfamiliarity regarding the proper way (protocol) to add comments, or answers, or edits to your question. I was told that we could not answer our own question. Hence, in the last edit, I have explained what were the fixes needed to sort out my problem. thanks,

Comment by Mike Scheutzow on 2022-03-03:
It is perfectly fine to answer your own question, as long as you are not repeating someone else's answer.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

So, the problem for robotic arm moving to the goal pose has been solved. There were couple of fixes needed. The first one was correcting the joint origin tag in my xacro file as pointed by @gvdhoorn, and given in the comments. The second issue is that, in my code, move_group_interface is using world as reference frame, but, the goal pose is given in reference to the base_link. Looking at my xacro file, the base_link was at an offset about 0.5 meters w.r.t to the world frame along the z axis. So, I just added 0.5 to all the target pose values z-coordinate.

Originally posted by zahid990170 with karma: 135 on 2022-03-03

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by Mike Scheutzow on 2022-03-03:
Thanks for the clear explanation.

Improve this answer
$\endgroup$

Your Answer

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