Gazebo object spawned in robot link frame does not move with robot link

asked 2016-07-30 16:04:38 -0500

jwatson_utah_edu gravatar image

updated 2016-07-30 18:13:25 -0500

ROS Indigo on Ubuntu 14.04 with Gazebo 7.3 and Python 2.7

I use a Gazebo service to spawn a fixed object in a link reference frame, but the object remains fixed with respect to the world instead of with respect to the frame. Why is this so? Here is the rospy code:

def test_robot_attachment():
    """ Try attaching an axes model to a robot frame and see what happens """
    global attachCounter
    spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) # Get a handle to the Gazebo model spawner
    modelPath = rospack.get_path('grasp_anything') + '/urdf/block_axes.urdf' # Build a path to the model URDF
    f = open(modelPath,'r') # Open the URDF for reading
    sdff = f.read() # and read it
    rospy.wait_for_service('gazebo/spawn_sdf_model') # Wait for the model loader to be ready
    targetPose = ROS_geo_pose_from_lists( [ 0.5 , 0.5 , 0.5 ] , [0,0,0,1] )
    spawn_model_prox("axesAttached" + str(attachCounter), sdff, "robotos_name_space", targetPose, "lbr4_allegro::lbr4_7_link") # gen model name and place the axes at the specfied pose
    attachCounter += 1

Is there a way to attach a model to a moving frame such that it moves with the frame but is static within the frame itself? As you can see below, the URDF being inserted has no collision geometry, the model is set to static, and gravity is turned off for the model.

Here is "block_axes.urdf":

<?xml version="1.0"?>
<!-- URL, Spawning simple geometry in Gazebo: http://wiki.ros.org/simulator_gazebo/Tutorials/SpawningObjectInSimulation -->
<robot name="block_axes"> <!-- Every URDF model you add is a "robot" -->
    <link name="x_axis">
        <!-- URL, Inertia element must be present: http://gazebosim.org/tutorials?tut=ros_urdf#%3Cinertial%3EElement -->
        <inertial>
            <origin xyz="2 0 0" />
            <mass value="1.0" />
            <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="100.0"  iyz="0.0"  izz="1.0" />
        </inertial>
        <visual>
            <!--origin xyz="2 0 1"/-->  <!-- Is this absolute or relative? -->
            <origin xyz="0.05 0 0"/>
            <geometry>
                <box size="0.1 0.01 0.01" />
            </geometry>
        </visual>
    </link>
    <joint name="X-Y" type="continuous">
        <parent link="x_axis"/>
        <child  link="y_axis"/>
    </joint>
    <link name="y_axis">
        <!-- URL, Inertia element must be present: http://gazebosim.org/tutorials?tut=ros_urdf#%3Cinertial%3EElement -->
        <inertial>
            <origin xyz="2 0 0" />
            <mass value="1.0" />
            <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="100.0"  iyz="0.0"  izz="1.0" />
        </inertial>
        <visual>
            <!--origin xyz="2 0 1"/-->  <!-- Is this absolute or relative? -->
            <origin xyz="0 0.05 0"/>
            <geometry>
                <box size="0.01 0.1 0.01" />
            </geometry>
        </visual>
    </link>
    <joint name="X-Z" type="continuous">
        <parent link="x_axis"/>
        <child  link="z_axis"/>
    </joint>
    <link name="z_axis">
        <!-- URL, Inertia element must be present: http://gazebosim.org/tutorials?tut=ros_urdf#%3Cinertial%3EElement -->
        <inertial>
            <origin xyz="2 0 0" />
            <mass value="1.0" />
            <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="100.0"  iyz="0.0"  izz="1.0" />
        </inertial>
        <visual>
            <!--origin xyz="2 0 1"/-->  <!-- Is this absolute or relative? -->
            <origin xyz="0 0 0.05"/>
            <geometry>
                <box size="0.01 0.01 0.1" />
            </geometry>
        </visual>
    </link>
    <gazebo reference ...
(more)
edit retag flag offensive close merge delete

Comments

My present goal is to attach and detach a physics object to and from a particular robot link. I need something even "cheaper" than the gazebo_grasp_plugin, as I wish to avoid all contact modeling, and gazebo_grasp_plugin, does not allow joints in between palm and gripper tips.

jwatson_utah_edu gravatar image jwatson_utah_edu  ( 2016-07-30 16:11:25 -0500 )edit