[how to] Crank mechanism under URDF notation.

edit

There is no easy way to model this in URDF. The problem is that you only have one independent joint here, the one that connects the crank to the fixed frame. The rest of the angles (crank to lever, lever to block) are dependent on the first angle.

There are no set ways to control dependent joints. There are some nodes that support use of a mimic tag, although the mimic tag is not well documented. However, my understanding is that it only supports offsets and multipliers, not trigonometric functions, and thus may not work for you.

If you are only looking to visualize this mechanism, you can manually publish the joint angles yourself. However, if you want to simulate it in Gazebo or do any number of other operations, it can get a bit trickier.

I made a simple URDF crank for you.

<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
     xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
     xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
     xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" name="crank">

    <xacro:property name="R" value="1.0" />
    <xacro:property name="L" value="2.0" />
    <xacro:property name="x" value="0.1" />
    <xacro:property name="b" value="0.5" />
    <xacro:property name="pi" value="3.1415" />

  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.05 .5 0.05"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
  </link>

  <link name="crank_link">
    <visual>
      <geometry>
        <box size="${R} ${x} ${x}"/>
      </geometry>
      <origin xyz="${-R/2} 0 0" />

      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
  </link>

  <joint name="theta" type="continuous">
    <parent link="base_link"/>
    <child link="crank_link"/>
    <axis xyz="0 ${R} 0"/>
  </joint>


  <link name="lever_link">
    <visual>
      <geometry>
        <box size="${L} ${x} ${x}"/>
      </geometry>
      <origin xyz="${-L/2} 0 0"/>

      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
  </link>

  <joint name="alpha" type="continuous">
    <parent link="crank_link"/>
    <child link="lever_link"/>
    <origin xyz="${-R} 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="box_link">
    <visual>
      <geometry>
        <box size="${b} ${b} ${b}"/>
      </geometry>
      <origin xyz="${-b/2} 0 0"/>
      <material name="red"/>
    </visual>
  </link>

  <joint name="psi" type="continuous">
    <parent link="lever_link"/>
    <child link="box_link"/>
    <origin xyz="${-L} 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

</robot>

#!/usr/bin/python

import roslib; roslib.load_manifest('crank')
import rospy
from sensor_msgs.msg import JointState
from math import pi, sin, asin
import math

R = 1.0
L = 2.0

if __name__ == '__main__':
    try:
        rospy.init_node('joint_state_publisher')
        pub = rospy.Publisher('joint_states', JointState)
        r = rospy.Rate(50) 
        theta = math.radians(45)

        # Publish Joint States
        while not rospy.is_shutdown():
            msg = JointState()
            msg.header.stamp = rospy.Time.now()
            msg.name = ['theta', 'alpha', 'psi']

            psi = asin( (R/L) * sin(theta) )
            alpha = - theta - psi

            msg.position = [theta, alpha, psi]
            print msg.position
            msg.velocity = [0]*3

            pub.publish(msg)

            theta += .1
            if theta >= 2 * pi:
                theta -= 2 * pi

            r.sleep()        
    except rospy.ROSInterruptException: pass