Commits

John Hsu committed 066159e

rudimentarily functional gearbox with example gearbox.world

Comments (0)

Files changed (6)

gazebo/physics/GearboxJoint.hh

     /// \{
 
     /// \class GearboxJoint GearboxJoint.hh physics/physics.hh
-    /// \brief A single axis hinge joint
+    /// \brief A double axis gearbox joint
     template<class T>
     class GearboxJoint : public T
     {
       /// \brief Constructor
       /// \param[in] _parent Parent link
       public: GearboxJoint(BasePtr _parent) : T(_parent)
-              { this->AddType(Base::HINGE_JOINT); }
+              { this->AddType(Base::GEARBOX_JOINT); }
       ///  \brief Destructor
       public: virtual ~GearboxJoint()
               { }
 
       /// \interal
       public: virtual unsigned int GetAngleCount() const
-              {return 1;}
+              {return 2;}
 
       /// \brief Load joint
       /// \param[in] _sdf Pointer to SDF element
       public: virtual void Load(sdf::ElementPtr _sdf)
               {
                 T::Load(_sdf);
+                if (_sdf->HasElement("gear_ratio"))
+                {
+                  this->gearRatio =
+                    _sdf->GetElement("gear_ratio")->GetValueDouble();
+                }
+                else
+                {
+                  gzerr << "should not see this\n";
+                  this->gearRatio = 1.0;
+                }
 
-                this->SetAxis(0,
-                    _sdf->GetElement("axis")->GetValueVector3("xyz"));
-
-                this->SetAxis(1,
-                    _sdf->GetElement("axis2")->GetValueVector3("xyz"));
               }
 
       /// \brief Initialize joint
-      // protected: virtual void Init()
-      //            {
-      //              T::Init();
-      //            }
+      protected: virtual void Init()
+                 {
+                   T::Init();
+                 }
+
+      /// \brief Set gearbox joint gear ratio.
+      ///
+      /// This must be implemented in a child class
+      /// \param[in] _index Index of the axis.
+      /// \param[in] _gearRatio Gear ratio value.
+      public: virtual void SetGearRatio(double _gearRatio) = 0;
+
+      /// \brief Gearbox gearRatio
+      protected: double gearRatio;
     };
     /// \}
   }

gazebo/physics/Joint.cc

 //////////////////////////////////////////////////
 void Joint::Init()
 {
+  gzerr << "attach\n";
+
   this->Attach(this->parentLink, this->childLink);
 
   // Set the anchor vector
     _msg.add_angle(this->GetAngle(1).Radian());
   }
 
-  msgs::Set(_msg.mutable_axis1()->mutable_xyz(), this->GetGlobalAxis(0));
+  // msgs::Set(_msg.mutable_axis1()->mutable_xyz(), this->GetGlobalAxis(0));
+  msgs::Set(_msg.mutable_axis1()->mutable_xyz(), math::Vector3());
   _msg.mutable_axis1()->set_limit_lower(0);
   _msg.mutable_axis1()->set_limit_upper(0);
   _msg.mutable_axis1()->set_limit_effort(0);

gazebo/physics/ode/ODEGearboxJoint.cc

 {
   GearboxJoint<ODEJoint>::Load(_sdf);
 
-  this->SetParam(dParamFMax, 0);
-  this->SetForce(0, 0);
+  this->SetGearRatio(this->gearRatio);
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::SetGearRatio(double _gearRatio)
+{
+  dJointSetGearboxRatio(this->jointId, _gearRatio);
 }
 
 //////////////////////////////////////////////////
 //////////////////////////////////////////////////
 void ODEGearboxJoint::SetAnchor(int _index, const math::Vector3 &_anchor)
 {
-  gzerr << "debug\n";
-
   if (this->childLink)
     this->childLink->SetEnabled(true);
   if (this->parentLink)

gazebo/physics/ode/ODEGearboxJoint.hh

       public: virtual void SetDamping(int _index, double _damping);
 
       // Documentation inherited
+      public: virtual void SetGearRatio(double _gearRatio);
+
+      // Documentation inherited
       public: virtual math::Angle GetAngleImpl(int _index) const;
 
       // Documentation inherited

gazebo/sdf/1.3/joint.sdf

     <description>offset from child link origin in child link frame.</description>
   </element>
 
+  <element name="gear_ratio" type="double" default="1.0" required="0">
+    <description>Parameter for gearbox joints.</description>
+  </element>
+
   <element name="thread_pitch" type="double" default="1.0" required="0">
-    <description></description>
+    <description>Parameter for screw joints.</description>
   </element>
 
   <element name="axis" required="1">

sdf/worlds/gearbox.world

               <xyz>1.000000 0.000000 0.000000</xyz>
             </axis>
         </joint>
-        <joint name="joint_1" type="gearbox">
+        <joint name="joint_1" type="revolute">
             <parent>link_1</parent>
             <child>link_2</child>
             <axis>
               <xyz>1.000000 0.000000 0.000000</xyz>
             </axis>
+        </joint>
+        <joint name="joint_12" type="gearbox">
+            <parent>link_1</parent>
+            <child>link_2</child>
+            <pose>0 0 0 0 0 0</pose>
+            <gear_ratio>-2</gear_ratio>
+            <axis>
+              <xyz>1.000000 0.000000 0.000000</xyz>
+            </axis>
             <axis2>
               <xyz>1.000000 0.000000 0.000000</xyz>
             </axis2>