Revision history [back]

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
            psi2 = psi

            msg.position = [theta, alpha, psi2]
            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

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
            psi2 = psi

            msg.position = [theta, alpha, psi2]
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