1. OpenSourceRoboticsFoundation
  2. Untitled project
  3. gazebo

Commits

John Hsu  committed 190c572

add reading of reference body

  • Participants
  • Parent commits 98acf8a
  • Branches ode_gearbox

Comments (0)

Files changed (8)

File deps/opende/include/ode/objects.h

View file
 ODE_API void dJointSetGearboxRatio( dJointID j, dReal value );
 
 /**
+ * @brief set gearbox reference body
+ * @remarks
+ * This is used to get the joint angle of the two bodies, so
+ * one can enforce error correction on gear angles.
+ * @ingroup joints
+ */
+ODE_API void dJointSetGearboxReferenceBody( dJointID j, dBodyID b );
+
+/**
  * @brief get gearbox ratio
  * @ingroup joints
  */

File deps/opende/src/joints/gearbox.cpp

View file
                        result);
 }
 
+void dJointSetGearboxReferenceBody( dJointID j, dBodyID b )
+{
+    dxJointGearbox* joint = dynamic_cast<dxJointGearbox*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    joint->refBody = b;
+}
+
 void dJointSetGearboxRatio( dJointID j, dReal value )
 {
     dxJointGearbox* joint = dynamic_cast<dxJointGearbox*>(j);

File deps/opende/src/joints/gearbox.h

View file
     dReal ratio;        // gearbox ratio
     dReal erp;          // error reduction
     dReal cfm;          // constraint force mix in
+    dxBody *refBody;    // reference body for calculating gear angles
     
     dxJointGearbox(dxWorld *w);
 

File gazebo/physics/GearboxJoint.hh

View file
                   this->gearRatio = 1.0;
                 }
 
+                if (_sdf->HasElement("reference_body"))
+                {
+                  this->referenceBody =
+                    _sdf->GetElement("reference_body")->GetValueString();
+                }
+                else
+                {
+                  gzerr << "Gearbox joint missing reference body.\n";
+                }
+
               }
 
       /// \brief Initialize joint
 
       /// \brief Gearbox gearRatio
       protected: double gearRatio;
+
+      /// \brief reference link/body for computing joint angles
+      protected: std::string referenceBody;
     };
     /// \}
   }

File gazebo/physics/ode/ODEGearboxJoint.cc

View file
 
 #include <boost/bind.hpp>
 
-#include "gazebo_config.h"
-#include "common/Console.hh"
+#include "gazebo/gazebo_config.h"
+#include "gazebo/common/Console.hh"
 
-#include "physics/Link.hh"
-#include "physics/ode/ODEGearboxJoint.hh"
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/ode/ODELink.hh"
+#include "gazebo/physics/ode/ODEGearboxJoint.hh"
 
 using namespace gazebo;
 using namespace physics;
   GearboxJoint<ODEJoint>::Load(_sdf);
 
   this->SetGearRatio(this->gearRatio);
+  LinkPtr link = this->model->GetLink(this->referenceBody);
+  if (link)
+    this->SetReferenceBody(link);
+}
+
+void ODEGearboxJoint::SetReferenceBody(LinkPtr _body)
+{
+  ODELinkPtr odelink = boost::shared_dynamic_cast<ODELink>(_body);
+
+  if (odelink == NULL)
+    gzwarn << "Reference body not valid, using inertial frame.\n";
+  else
+    dJointSetGearboxReferenceBody(this->jointId, odelink->GetODEId());
 }
 
 //////////////////////////////////////////////////

File gazebo/physics/ode/ODEGearboxJoint.hh

View file
       public: virtual void SetGearRatio(double _gearRatio);
 
       // Documentation inherited
+      public: virtual void SetReferenceBody(LinkPtr _body);
+
+      // Documentation inherited
       public: virtual math::Angle GetAngleImpl(int _index) const;
 
       // Documentation inherited

File gazebo/sdf/1.3/joint.sdf

View file
     <description>Parameter for gearbox joints.</description>
   </element>
 
+  <element name="reference_body" type="string" default="__default__" required="0">
+    <description>Parameter for gearbox joints.</description>
+  </element>
+
   <element name="thread_pitch" type="double" default="1.0" required="0">
     <description>Parameter for screw joints.</description>
   </element>

File sdf/worlds/gearbox.world

View file
             <kinematic>0</kinematic>
         </link>
         <link name="link_2">
+            <pose>0.500000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+            <inertial>
+                <pose>0.000000 0.000000 -0.500000 0.000000 -0.000000 0.000000</pose>
+                <inertia>
+                    <ixx>1.000000</ixx>
+                    <ixy>0.000000</ixy>
+                    <ixz>0.000000</ixz>
+                    <iyy>1.000000</iyy>
+                    <iyz>0.000000</iyz>
+                    <izz>1.000000</izz>
+                </inertia>
+                <mass>10.000000</mass>
+            </inertial>
+            <visual name="visual_cylinder">
+                <pose>0.000000 0.000000 -0.500000 0.000000 -0.000000 0.000000</pose>
+                <geometry>
+                    <cylinder>
+                        <radius>0.100000</radius>
+                        <length>1.000000</length>
+                    </cylinder>
+                </geometry>
+                <material>
+                    <script>Gazebo/Green</script>
+                </material>
+                <cast_shadows>1</cast_shadows>
+                <laser_retro>100.000000</laser_retro>
+                <transparency>0.000000</transparency>
+            </visual>
+            <collision name="collision_cylinder">
+                <pose>0.000000 0.000000 -0.500000 0.000000 -0.000000 0.000000</pose>
+                <max_contacts>250</max_contacts>
+                <geometry>
+                    <cylinder>
+                        <radius>0.100000</radius>
+                        <length>1.000000</length>
+                    </cylinder>
+                </geometry>
+            </collision>
+            <gravity>1</gravity>
+            <self_collide>1</self_collide>
+            <kinematic>0</kinematic>
+        </link>
+        <link name="link_3">
             <pose>1.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
             <inertial>
                 <pose>0.000000 0.000000 -0.500000 0.000000 -0.000000 0.000000</pose>
             <self_collide>1</self_collide>
             <kinematic>0</kinematic>
         </link>
-        <joint name="joint_0" type="revolute">
+        <joint name="joint_02" type="revolute">
             <parent>world</parent>
+            <child>link_2</child>
+            <axis>
+              <xyz>1.000000 0.000000 0.000000</xyz>
+            </axis>
+        </joint>
+        <joint name="joint_12" type="revolute">
+            <parent>link_2</parent>
             <child>link_1</child>
             <axis>
               <xyz>1.000000 0.000000 0.000000</xyz>
             </axis>
         </joint>
-        <joint name="joint_1" type="revolute">
-            <parent>link_1</parent>
-            <child>link_2</child>
+        <joint name="joint_23" type="revolute">
+            <parent>link_2</parent>
+            <child>link_3</child>
             <axis>
               <xyz>1.000000 0.000000 0.000000</xyz>
             </axis>
         </joint>
-        <joint name="joint_12" type="gearbox">
+        <joint name="joint_13" type="gearbox">
             <parent>link_1</parent>
-            <child>link_2</child>
+            <child>link_3</child>
             <pose>0 0 0 0 0 0</pose>
             <gear_ratio>-10</gear_ratio>
+            <reference_body>link_2</reference_body>
             <axis>
               <xyz>1.000000 0.000000 0.000000</xyz>
             </axis>