[Personalrobots-commit] SF.net SVN: personalrobots:[3502] pkg/trunk

[<stu...@us...>]

Revision: 3502
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3502&view=rev
Author:   stuglaser
Date:     2008-08-22 22:38:24 +0000 (Fri, 22 Aug 2008)

Log Message:
-----------
Made the torso joint prismatic, and handled prismatic (slider) joints in the gazebo plugin.

Modified Paths:
--------------
    pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp	2008-08-22 22:37:58 UTC (rev 3501)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp	2008-08-22 22:38:24 UTC (rev 3502)
@@ -141,11 +141,26 @@
   {
     if (!joints_[i])
       continue;
-    assert(joints_[i]->GetType() == Joint::HINGE);
-    HingeJoint *hj = (HingeJoint*)joints_[i];
-    fake_model_.joints_[i]->position_ = hj->GetAngle();
-    fake_model_.joints_[i]->velocity_ = hj->GetAngleRate();
+
     fake_model_.joints_[i]->applied_effort_ = fake_model_.joints_[i]->commanded_effort_;
+
+    switch(joints_[i]->GetType())
+    {
+    case Joint::HINGE: {
+      HingeJoint *hj = (HingeJoint*)joints_[i];
+      fake_model_.joints_[i]->position_ = hj->GetAngle();
+      fake_model_.joints_[i]->velocity_ = hj->GetAngleRate();
+      break;
+    }
+    case Joint::SLIDER: {
+      SliderJoint *sj = (SliderJoint*)joints_[i];
+      fake_model_.joints_[i]->position_ = sj->GetPosition();
+      fake_model_.joints_[i]->velocity_ = sj->GetPositionRate();
+      break;
+    }
+    default:
+      abort();
+    }
   }
 
   // Reverses the transmissions to propagate the joint position into the actuators.
@@ -171,9 +186,19 @@
   {
     if (!joints_[i])
       continue;
-    assert(joints_[i]->GetType() == Joint::HINGE);
-    HingeJoint *hj = (HingeJoint*)joints_[i];
-    hj->SetTorque(fake_model_.joints_[i]->commanded_effort_);
+
+    double effort = fake_model_.joints_[i]->commanded_effort_;
+    switch (joints_[i]->GetType())
+    {
+    case Joint::HINGE:
+      ((HingeJoint*)joints_[i])->SetTorque(effort);
+      break;
+    case Joint::SLIDER:
+      ((SliderJoint*)joints_[i])->SetSliderForce(effort);
+      break;
+    default:
+      abort();
+    }
   }
 }
 

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml	2008-08-22 22:37:58 UTC (rev 3501)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml	2008-08-22 22:38:24 UTC (rev 3502)
@@ -794,7 +794,7 @@
   <joint name="shoulder_pan_left_joint" type="revolute" >
     <insert_const_block name="pr2_shoulder_pan_joint"/>
   </joint>
-  <joint name="torso_joint"> 
+  <joint name="torso_joint" type="prismatic"> 
     <limit min="0.0" max="torso_max_travel_range" effort="100" velocity="5" />
     <axis xyz="0 0 1" />
   </joint>


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.