Slow strong prismatic joint

asked 2018-04-26 08:28:56 -0600

nordmo1 gravatar image

Hi.

I'm having some problems modelling a slow and strong prismatic joint (real world screw joint). The problem seems to be that <velocity> is not being considered.

My joint looks like:

<xacro:macro name="small_extender" params="name rot:=1">
        <xacro:small_ext name="${name}_inner" />
        <xacro:small_ext2 name="${name}_outer" rot="${rot}" />
        <joint name="${name}" type="prismatic">
            <parent link="${name}_inner" />
            <child link="${name}_outer" />
            <axis xyz="0 0 1" />
            <origin xyz="0 0 ${small_ext_height - 0.026}" />
            <limit
                lower="-0.025" upper="0.0"
                effort="100.0" velocity="0.00045" />
            <dynamics damping="1.0" friction="80" />
        </joint>
        <gazebo reference="${name}">
            <implicitSpringDamper>1</implicitSpringDamper>
        </gazebo>
    </xacro:macro>

And I'm controlling the joint through a Gazebo plugin that uses JointController and a position PID with SetPositionTarget. Currently I'm using <pid>10000.0 0 0 </pid> for parameters because the distance is quite small. When I set a position target for this joint it moves a lot faster than the <velocity> tag indicates, and it seems to be the case however I tune the PID. The rest of the robot should have the correct masses so I don't think that is the problem.

edit retag flag offensive close merge delete