algo camina

src/simple_biped/leg_visual.py

@@ -28,7 +28,7 @@
 def periodize(t,spline_parameters):
     real_t = t % 2
     print real_t
-    return splev(real_t+4,spline_parameters) 
+    return splev(real_t+4,spline_parameters) + 0.05*splev(real_t+4,spline_parameters,der=1)
 
 class LegSkill(object):
     """
@@ -159,15 +159,15 @@ def __init__(self):
     for i,thetha in enumerate(ankle_angles):
         ankle_angles[i] = (thetha-180)*(3.14/180) 
     for i,thetha in enumerate(phalange_angles):
-        phalange_angles[i] = -100*3.14/180#((thetha)-180)*(3.14/180) #-90*3.14/180#
+        phalange_angles[i] = -70*3.14/180#((thetha)-180)*(3.14/180) #
     for i,thetha in enumerate(thigh_angles_c):
         thigh_angles_c[i] = -1*(thetha+90)*(3.14/180) 
     for i,thetha in enumerate(tibia_angles_c):
         tibia_angles_c[i] = (180-thetha)*(3.14/180)     
     for i,thetha in enumerate(ankle_angles_c):
         ankle_angles_c[i] = (thetha-180)*(3.14/180) 
     for i,thetha in enumerate(phalange_angles_c):
-        phalange_angles_c[i] = -100*3.14/180#((thetha)-180)*(3.14/180) #-90*3.14/180#
+        phalange_angles_c[i] = -70*3.14/180#-((thetha)-180)*(3.14/180) #
     l_leg = LeftLegSkill()
     r_leg = RightLegSkill()
 

urdf/leg.xacro

@@ -21,14 +21,14 @@
   <xacro:property name="ankle_height" value="0.0745" />
  <xacro:property name="ankle_width" value="0.020" />
  <xacro:property name="ankle_large" value="0.020" />
-   <xacro:property name="phalange_height" value="0.0745" />
+   <xacro:property name="phalange_height" value="0.1" />
  <xacro:property name="phalange_width" value="0.020" />
  <xacro:property name="phalange_large" value="0.020" />
    <!-- Friction model from "Characterization and modeling of a Dynamixel servo" by Arno Mensin (model AX12, better than nothing) -->
   <!-- Damping N.m.s/rad --> 
-  <xacro:property name="dxl_damping" value="0.15" />
+  <xacro:property name="dxl_damping" value="0.85" />
   <!-- Friccion N.m -->
-  <xacro:property name="dxl_friction" value="0.3" />
+  <xacro:property name="dxl_friction" value="0.9" />
   <xacro:macro name="cuboid_inertia_def" params="x y z mass">
     <inertia 
       iyy="${mass*(z*z+x*x)/12.0}"
@@ -150,9 +150,9 @@
 
   <link name="${side}_phalange" >
     <inertial>
-      <origin xyz="0 0 0" />
-      <mass value="0.07"/>
-      <xacro:cuboid_inertia_def x="${phalange_large}" y="${phalange_width}" z="${phalange_height}" mass="0.07"/>
+      <origin xyz="0 0 ${-1*phalange_height/2}" />
+      <mass value="2"/>
+      <xacro:cuboid_inertia_def x="${phalange_large}" y="${phalange_width}" z="${phalange_height}" mass="2"/>
     </inertial>
     <visual>
       <origin xyz="0 0 ${-1*phalange_height/2}" rpy="0 0 0" />
@@ -182,7 +182,7 @@
     <origin xyz="${-body_large/6} ${reflect*body_width/2} ${body_height/5}" rpy="0 0 0" />
     <axis xyz="0 0 1" />
     <!--limit effort="4" velocity="4" lower="0" upper="0.01" /-->
-    <!--dynamics damping="${dxl_damping}" friction="${dxl_friction}" /-->
+    <dynamics damping="${dxl_damping}" friction="${dxl_friction}" />
   </joint>
 
   <joint name="${side}_thigh_yaw_joint" type="fixed">
@@ -200,7 +200,7 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 1 0" />
     <limit effort="8" velocity="6" lower="${-75*M_PI/180}" upper="${45*M_PI/180}"  />
-    <!--dynamics damping="${dxl_damping}" friction="${dxl_friction}" /-->
+    <dynamics damping="${dxl_damping}" friction="${dxl_friction}" />
   </joint>
 
   <joint name="${side}_tibia_joint" type="revolute">
@@ -209,7 +209,7 @@
     <origin xyz="0 0 ${-1*thigh_height}" rpy="0 0 0" />
     <axis xyz="0 1 0" />
     <limit effort="8" velocity="6" lower="${0*M_PI/180}" upper="${150*M_PI/180}" />
-    <!--dynamics damping="${dxl_damping}" friction="${dxl_friction}" /-->
+    <dynamics damping="${dxl_damping}" friction="${dxl_friction}" />
   </joint>
 
   <joint name="${side}_ankle_joint" type="revolute">
@@ -218,7 +218,7 @@
     <origin xyz="0 0 ${-1*tibia_height}" rpy="0 0 0" />
     <axis xyz="0 1 0" />
     <limit effort="8" velocity="6" lower="-2.1" upper="-0.8" />
-    <!--dynamics damping="${dxl_damping}" friction="${dxl_friction}" /-->
+    <dynamics damping="${dxl_damping}" friction="${dxl_friction}" />
   </joint>
 
   <joint name="${side}_phalange_joint" type="revolute">
@@ -227,7 +227,7 @@
     <origin xyz="0 0 ${-1*ankle_height}" rpy="0 0 0" />
     <axis xyz="0 1 0" />
     <limit effort="8" velocity="6" lower="${-45*M_PI/180}" upper="0" />
-    <!--dynamics damping="${dxl_damping}" friction="${dxl_friction}" /-->
+    <dynamics damping="${dxl_damping}" friction="${dxl_friction}" />
   </joint>
 </xacro:macro>
 </robot>