adds an example of a cable with tension holding up a mass

doc/drakeURDF.xsd

@@ -113,6 +113,7 @@ These forces are represented by a 6x6 matrix 'Ma' similar to a mass matrix, wher
                     <xs:documentation> Adds a buoyancy force to the parent body, placed at a defined center of buoyancy. </xs:documentation>
                 </xs:annotation>
             </xs:element>
+            <xs:element name="torsional_spring" type="torsional_spring"/>
         </xs:choice>
         <xs:attribute name="name" type="xs:string"/>
     </xs:complexType>

examples/SimplePulleys/tension.m

@@ -0,0 +1,34 @@
+function tension
+
+% note that the masses are constrained to move only vertically
+r = PlanarRigidBodyManipulator('tension.urdf');
+
+v = r.constructVisualizer();
+v.xlim = [-5 5];
+v.ylim = [-.2 6.2];
+
+x0 = Point(getStateFrame(r));
+x0.load_x = 0;
+x0.load_z = 2;
+
+v.drawWrapper(0,x0(1:3));
+
+%[l,dl]=r.position_constraints{1}.eval(x0(1:3))
+r.position_constraints{1}.checkGradient(.001,x0(1:3));
+
+x0 = resolveConstraints(r,x0,v);
+v.drawWrapper(0,x0(1:3));
+
+ytraj = simulate(r,[0 1],x0);
+if(0)
+ts = ytraj.getBreaks();
+for i=1:numel(ts)
+  x = ytraj.eval(ts(i));
+  length(i) = r.position_constraints{1}.eval(x(1:3));
+end
+figure(1); clf; plot(ts,length);
+end
+
+v.playback(ytraj,struct('slider',true));
+
+

examples/SimplePulleys/tension.urdf

@@ -0,0 +1,102 @@
+
+<robot xmlns="http://drake.mit.edu"
+ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:schemaLocation="http://drake.mit.edu ../../doc/drakeURDF.xsd" name="SimplePulley">
+
+  <link name="base">
+    <visual>
+      <material>
+        <color rgba="1 .5 0 0"/>
+      </material>
+      <origin xyz="-3 1 4" rpy="1.57 0 0"/>
+      <geometry>
+        <cylinder radius=".01" length=".2"/>
+      </geometry>
+    </visual>
+    <visual>
+      <origin xyz="0 0 3"/>
+      <geometry>
+        <box size="9 1.7 6"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <link name="tensioner">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="0.065833" ixy="0.000000" ixz="0.000000" iyy="0.125" iyz="0.000000" izz="0.065833" />
+    </inertial>
+    <visual>
+      <material>
+        <color rgba="1 .5 0 0"/>
+      </material>
+      <origin xyz="0 0 0" rpy="1.57 0 0"/>
+      <geometry>
+        <cylinder radius=".5" length=".2"/>
+      </geometry>
+    </visual>
+    <visual> <!-- add a "dot" for reference -->
+      <material>
+        <color rgba="0 0 0 0"/>
+      </material>
+      <origin xyz="0 .2 .4" rpy="1.57 0 0"/>
+      <geometry>
+        <cylinder radius=".05" length=".2"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <link name="load">
+    <inertial>
+      <mass value="2"/>
+      <inertia ixx="0.506667" ixy="0.000000" ixz="0.000000" iyy="1.000000" iyz="0.000000" izz="0.506667" />
+    </inertial>
+    <visual>
+      <material>
+        <color rgba=".8 0 0 0"/>
+      </material>
+      <origin xyz="0 0 0" rpy="1.57 0 0"/>
+      <geometry>
+        <cylinder radius="1" length=".2"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <joint name="tensioner_angle" type="revolute">
+    <origin xyz="3 1 3.5"/>
+    <axis xyz="0 1 0"/>
+    <parent link="base"/>
+    <child link="tensioner"/>
+    <dynamics damping="2"/>
+  </joint>
+
+  <link name="load_x" />
+  <joint name="load_x" type="prismatic">
+    <origin xyz="0 1 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="base"/>
+    <child link="load_x"/>
+    <dynamics damping="2"/>
+  </joint>
+  <joint name="load_z" type="prismatic">
+    <origin xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="load_x"/>
+    <child link="load"/>
+    <dynamics damping="2"/>
+  </joint>
+
+  <force_element name="tensioner_spring">
+    <torsional_spring rest_angle="0" stiffness="10">
+      <joint name="tensioner_angle"/>
+    </torsional_spring>
+  </force_element>
+
+  <cable min_length="8" max_length="8">
+    <terminator link="base" xyz="-3 1 4" />
+    <pulley link="load" xyz="0 0 0" axis="0 -1 0" radius="1"/>
+    <pulley link="base" xyz="3 1 3.5" axis="0 1 0" radius=".5"/>
+    <terminator link="tensioner" xyz="0 0 .51"/>
+  </cable>
+
+</robot>

solvers/+drakeFunction/+kinematic/CableAndPulleys.m

@@ -40,6 +40,7 @@
 
           r1 = obj.pulley(i-1).radius;
           r2 = obj.pulley(i).radius;
+          assert(C>r1+r2+eps); % cut me a little slack, eh?
           if r1>0 || r2>0,            
             alignment = dot(obj.pulley(i-1).axis,obj.pulley(i).axis);
             if r1>0 && r2>0, % then make sure the axes are aligned
@@ -50,8 +51,8 @@
             cvec = vec/C;
 
             if alignment>0 % then it's like an open flat belt drive
-              s = (r2-r1)/C;
-              alpha = asin(s);
+              s = (r2-r1)/C;  
+              alpha = asin(s); 
               pt1 = last_pt + r1*axis2rotmat([obj.pulley(i-1).axis;-pi/2-alpha])*cvec;
               pt2 = pt + r2*axis2rotmat([obj.pulley(i).axis;-pi/2-alpha])*cvec;
               if nargout>1
@@ -100,7 +101,7 @@
 
             if nargout>1
               dvec = dpt2 - dpt1;
-              dC = vec'*dvec/C;
+              dC = vec'*dvec/(C+eps);
               dlength = dlength+dC;
             end
 

util/inertiaCalculator.m

@@ -0,0 +1,24 @@
+function I = inertiaCalculator(type,mass,varargin)
+% Usage
+%  inertiaCalculator('box',mass,size)
+%  inertiaCalculator('sphere',mass,radius)
+%  inertiaCalculator('cylinder',mass,radius,length)
+% see e.g. http://en.wikipedia.org/wiki/List_of_moments_of_inertia
+
+switch(type)
+  case 'box'
+    size=varargin{1};
+    I = 1/12*mass*diag([size(2)^2+size(3)^2,size(1)^2+size(3)^2,size(1)^2+size(2)^2]);
+  case 'sphere'
+    radius=varargin{1};
+    I = eye(3)*2*mass*radius^2/5;
+  case 'cylinder'
+    radius=varargin{1};
+    length=varargin{2};
+    ixx = 1/12*mass*(3*radius^2 + length^2);
+    I = diag([ixx,ixx,mass*radius^2/2]);
+end
+
+if nargout<1
+  fprintf('ixx="%f" ixy="%f" ixz="%f" iyy="%f" iyz="%f" izz="%f"\n',I(1,1),I(1,2),I(1,3),I(2,2),I(2,3),I(3,3));
+end