Commits

John Hsu  committed b55382e

still adding

  • Participants
  • Parent commits d9f94ec
  • Branches ode_gearbox

Comments (0)

Files changed (11)

File gazebo/gui/CMakeLists.txt

                                 ${QT_LIBRARIES}
                                 ${ogre_libraries}
                                 ${Boost_LIBRARIES}
+                                QtTest
                                 pthread)
 
 

File gazebo/physics/Base.hh

                 SCREW_JOINT     = 0x00000800,
                 /// \brief UniversalJoint type
                 UNIVERSAL_JOINT = 0x00001000,
+                /// \brief GearboxJoint type
+                GEARBOX_JOINT     = 0x00002000,
 
                 /// \brief Shape type
                 SHAPE           = 0x00002000,

File gazebo/physics/CMakeLists.txt

   HeightmapShape.hh
   Hinge2Joint.hh
   HingeJoint.hh
+  GearboxJoint.hh
   Inertial.hh
   Gripper.hh
   Joint.hh

File gazebo/physics/GearboxJoint.hh

+/*
+ * Copyright 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+/* Desc: A body that has a box shape
+ * Author: Nate Koenig, Andrew Howard
+ * Date: 21 May 2003
+ */
+
+#ifndef _GEARBOXJOINT_HH_
+#define _GEARBOXJOINT_HH_
+
+#include "gazebo/math/Angle.hh"
+#include "gazebo/math/Vector3.hh"
+
+namespace gazebo
+{
+  namespace physics
+  {
+    /// \addtogroup gazebo_physics
+    /// \{
+
+    /// \class GearboxJoint GearboxJoint.hh physics/physics.hh
+    /// \brief A single axis hinge 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); }
+      ///  \brief Destructor
+      public: virtual ~GearboxJoint()
+              { }
+
+      /// \interal
+      public: virtual unsigned int GetAngleCount() const
+              {return 1;}
+
+      /// \brief Load joint
+      /// \param[in] _sdf Pointer to SDF element
+      public: virtual void Load(sdf::ElementPtr _sdf)
+              {
+                T::Load(_sdf);
+
+                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();
+      //            }
+    };
+    /// \}
+  }
+}
+#endif
+

File gazebo/physics/ode/CMakeLists.txt

              ODEJoint.cc
              ODESliderJoint.cc
              ODEHingeJoint.cc
+             ODEGearboxJoint.cc
              ODEHinge2Joint.cc
              ODEUniversalJoint.cc
              ODEBallJoint.cc
   ODEJoint.hh
   ODESliderJoint.hh
   ODEHingeJoint.hh
+  ODEGearboxJoint.hh
   ODEHinge2Joint.hh
   ODEUniversalJoint.hh
   ODEBallJoint.hh

File gazebo/physics/ode/ODEGearboxJoint.cc

+/*
+ * Copyright 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+/* Desc: A ODEGearboxJoint
+ * Author: Nate Koenig, Andrew Howard
+ * Date: 21 May 2003
+ */
+
+#include <boost/bind.hpp>
+
+#include "gazebo_config.h"
+#include "common/Console.hh"
+
+#include "physics/Link.hh"
+#include "physics/ode/ODEGearboxJoint.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+ODEGearboxJoint::ODEGearboxJoint(dWorldID _worldId, BasePtr _parent)
+    : GearboxJoint<ODEJoint>(_parent)
+{
+  this->jointId = dJointCreateGearbox(_worldId, NULL);
+}
+
+//////////////////////////////////////////////////
+ODEGearboxJoint::~ODEGearboxJoint()
+{
+  if (this->applyDamping)
+    physics::Joint::DisconnectJointUpdate(this->applyDamping);
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::Load(sdf::ElementPtr _sdf)
+{
+  GearboxJoint<ODEJoint>::Load(_sdf);
+
+  this->SetParam(dParamFMax, 0);
+  this->SetForce(0, 0);
+}
+
+//////////////////////////////////////////////////
+math::Vector3 ODEGearboxJoint::GetAnchor(int _index) const
+{
+  dVector3 result;
+
+  if (_index == 0)
+    dJointGetGearboxAxis1(this->jointId, result);
+  else if (_index == 1)
+    dJointGetGearboxAxis2(this->jointId, result);
+  else
+    gzerr << "requesting GetAnchor axis [" << _index << "] out of range\n";
+
+  return math::Vector3(result[0], result[1], result[2]);
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::SetAnchor(int _index, const math::Vector3 &_anchor)
+{
+  gzerr << "debug\n";
+
+  if (this->childLink)
+    this->childLink->SetEnabled(true);
+  if (this->parentLink)
+    this->parentLink->SetEnabled(true);
+
+  if (_index == 0)
+    dJointSetGearboxAxis1(this->jointId, _anchor.x, _anchor.y, _anchor.z);
+  else if (_index == 1)
+    dJointSetGearboxAxis2(this->jointId, _anchor.x, _anchor.y, _anchor.z);
+  else
+    gzerr << "requesting SetAnchor axis [" << _index << "] out of range\n";
+}
+
+
+//////////////////////////////////////////////////
+math::Vector3 ODEGearboxJoint::GetGlobalAxis(int _index) const
+{
+  dVector3 result;
+
+  if (_index == 0)
+    dJointGetGearboxAxis1(this->jointId, result);
+  else if (_index == 1)
+    dJointGetGearboxAxis2(this->jointId, result);
+  else
+    gzerr << "requesting GetAnchor axis [" << _index << "] out of range\n";
+
+  return math::Vector3(result[0], result[1], result[2]);
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::SetAxis(int _index, const math::Vector3 &_axis)
+{
+  gzerr << "debug\n";
+
+  if (this->childLink)
+    this->childLink->SetEnabled(true);
+  if (this->parentLink)
+    this->parentLink->SetEnabled(true);
+
+  if (_index == 0)
+    dJointSetGearboxAxis1(this->jointId, _axis.x, _axis.y, _axis.z);
+  else if (_index == 1)
+    dJointSetGearboxAxis2(this->jointId, _axis.x, _axis.y, _axis.z);
+  else
+    gzerr << "requesting SetAnchor axis [" << _index << "] out of range\n";
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::SetDamping(int /*index*/, double _damping)
+{
+  this->dampingCoefficient = _damping;
+  // use below when ode version is fixed
+  // dJointSetDamping(this->jointId, this->dampingCoefficient);
+  this->applyDamping = physics::Joint::ConnectJointUpdate(
+    boost::bind(&Joint::ApplyDamping, this));
+}
+
+//////////////////////////////////////////////////
+math::Angle ODEGearboxJoint::GetAngleImpl(int /*index*/) const
+{
+  math::Angle result;
+  if (this->jointId)
+    result = dJointGetHingeAngle(this->jointId);
+  return result;
+}
+
+//////////////////////////////////////////////////
+double ODEGearboxJoint::GetVelocity(int /*index*/) const
+{
+  double result = dJointGetHingeAngleRate(this->jointId);
+
+  return result;
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::SetVelocity(int /*index*/, double _angle)
+{
+  this->SetParam(dParamVel, _angle);
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::SetMaxForce(int /*index*/, double _t)
+{
+  return this->SetParam(dParamFMax, _t);
+}
+
+//////////////////////////////////////////////////
+double ODEGearboxJoint::GetMaxForce(int /*index*/)
+{
+  return this->GetParam(dParamFMax);
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::SetForce(int _index, double _torque)
+{
+  ODEJoint::SetForce(_index, _torque);
+  if (this->childLink)
+    this->childLink->SetEnabled(true);
+  if (this->parentLink)
+    this->parentLink->SetEnabled(true);
+  dJointAddHingeTorque(this->jointId, _torque);
+}
+
+//////////////////////////////////////////////////
+double ODEGearboxJoint::GetParam(int _parameter) const
+{
+  double result = dJointGetHingeParam(this->jointId, _parameter);
+
+  return result;
+}
+
+//////////////////////////////////////////////////
+void ODEGearboxJoint::SetParam(int _parameter, double _value)
+{
+  ODEJoint::SetParam(_parameter, _value);
+
+  dJointSetHingeParam(this->jointId, _parameter, _value);
+}

File gazebo/physics/ode/ODEGearboxJoint.hh

+/*
+ * Copyright 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+/* Desc: A ODE gearbox joint
+ * Author: Nate Koenig
+ * Date: 21 May 2003
+ */
+
+#ifndef _ODEGEARBOXJOINT_HH_
+#define _ODEGEARBOXJOINT_HH_
+
+#include "gazebo/math/Angle.hh"
+#include "gazebo/math/Vector3.hh"
+
+#include "gazebo/physics/GearboxJoint.hh"
+#include "gazebo/physics/ode/ODEJoint.hh"
+
+namespace gazebo
+{
+  namespace physics
+  {
+    /// \brief A double axis gearbox joint.
+    class ODEGearboxJoint : public GearboxJoint<ODEJoint>
+    {
+      /// \brief Constructor
+      /// \param[in] _worldID ODE id of the world.
+      /// \param[in] _parent Parent of the Joint
+      public: ODEGearboxJoint(dWorldID _worldId, BasePtr _parent);
+
+      /// \brief Destructor.
+      public: virtual ~ODEGearboxJoint();
+
+      // Documentation inherited
+      public: virtual void Load(sdf::ElementPtr _sdf);
+
+      // Documentation inherited
+      public: virtual math::Vector3 GetAnchor(int _index) const;
+
+      // Documentation inherited
+      public: virtual void SetAnchor(int _index, const math::Vector3 &_anchor);
+
+      // Documentation inherited
+      public: virtual math::Vector3 GetGlobalAxis(int _index) const;
+
+      // Documentation inherited
+      public: virtual void SetAxis(int _index, const math::Vector3 &_axis);
+
+      // Documentation inherited
+      public: virtual void SetDamping(int _index, double _damping);
+
+      // Documentation inherited
+      public: virtual math::Angle GetAngleImpl(int _index) const;
+
+      // Documentation inherited
+      public: virtual void SetVelocity(int _index, double _angle);
+
+      // Documentation inherited
+      public: virtual double GetVelocity(int _index) const;
+
+      // Documentation inherited
+      public: virtual void SetMaxForce(int _index, double _t);
+
+      // Documentation inherited
+      public: virtual double GetMaxForce(int _index);
+
+      // Documentation inherited
+      public: virtual void SetForce(int _index, double _torque);
+
+      // Documentation inherited
+      public: virtual double GetParam(int _parameter) const;
+
+      // Documentation inherited
+      public: virtual void SetParam(int _parameter, double _value);
+    };
+  }
+}
+#endif

File gazebo/physics/ode/ODEPhysics.cc

 #include "gazebo/physics/ode/ODELink.hh"
 #include "gazebo/physics/ode/ODEScrewJoint.hh"
 #include "gazebo/physics/ode/ODEHingeJoint.hh"
+#include "gazebo/physics/ode/ODEGearboxJoint.hh"
 #include "gazebo/physics/ode/ODEHinge2Joint.hh"
 #include "gazebo/physics/ode/ODESliderJoint.hh"
 #include "gazebo/physics/ode/ODEBallJoint.hh"
     joint.reset(new ODEScrewJoint(this->worldId, _parent));
   else if (_type == "revolute")
     joint.reset(new ODEHingeJoint(this->worldId, _parent));
+  else if (_type == "gearbox")
+    joint.reset(new ODEGearboxJoint(this->worldId, _parent));
   else if (_type == "revolute2")
     joint.reset(new ODEHinge2Joint(this->worldId, _parent));
   else if (_type == "ball")

File gazebo/sdf/1.3/joint.sdf

   </attribute>
 
   <attribute name="type" type="string" default="__default__" required="1">
-    <description>The type of joint, which must be one of the following: (revolute) a hinge joint that rotates on a single axis with either a fixed or continuous range of motion, (revolute2) same as two revolute joints connected in series, (prismatic) a sliding joint that slides along an axis with a limited range specified by upper and lower limits, (ball) a ball and socket joint, (universal), like a ball joint, but constrains one degree of freedom, (piston) similar to a Slider joint except that rotation around the translation axis is possible.</description>
+    <description>The type of joint, which must be one of the following: (revolute) a hinge joint that rotates on a single axis with either a fixed or continuous range of motion, (gearbox) geared revolute joints, (revolute2) same as two revolute joints connected in series, (prismatic) a sliding joint that slides along an axis with a limited range specified by upper and lower limits, (ball) a ball and socket joint, (universal), like a ball joint, but constrains one degree of freedom, (piston) similar to a Slider joint except that rotation around the translation axis is possible.</description>
   </attribute>
 
   <element name="parent" type="string" default="__default__" required="1">

File sdf/worlds/gearbox.world

+<?xml version="1.0" ?>
+<sdf version="1.3">
+  <world name="default">
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- A ground plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <physics type="ode">
+        <gravity>0.000000 0.000000 -9.810000</gravity>
+        <ode>
+            <solver>
+                <type>quick</type>
+                <dt>0.001000</dt>
+                <iters>100</iters>
+                <precon_iters>0</precon_iters>
+                <sor>1.300000</sor>
+            </solver>
+            <constraints>
+                <cfm>0.000000</cfm>
+                <erp>1.000000</erp>
+                <contact_max_correcting_vel>0.000000</contact_max_correcting_vel>
+                <contact_surface_layer>0.000000</contact_surface_layer>
+            </constraints>
+        </ode>
+        <update_rate>0.000000</update_rate>
+    </physics>
+
+    <model name="model_1">
+        <pose>0.000000 0.000000 2.100000 -1.415927 -0.000000 0.000000</pose>
+        <link name="link_1">
+            <pose>0.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>
+                <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_2">
+            <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>
+                <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>
+        <joint name="joint_0" type="revolute">
+            <parent>world</parent>
+            <child>link_1</child>
+            <axis>
+              <xyz>1.000000 0.000000 0.000000</xyz>
+            </axis>
+        </joint>
+        <joint name="joint_1" type="gearbox">
+            <parent>link_1</parent>
+            <child>link_2</child>
+            <axis>
+              <xyz>1.000000 0.000000 0.000000</xyz>
+            </axis>
+            <axis2>
+              <xyz>1.000000 0.000000 0.000000</xyz>
+            </axis2>
+        </joint>
+        <static>0</static>
+    </model>
+  </world>
+</sdf>

File tools/CMakeLists.txt

 gz_install_executable(gzsdf)
 
 add_executable(gztopic gztopic.cc)
-target_link_libraries(gztopic gazebo_msgs gazebo_common gazebo_transport pthread gazebo_gui ${QT_LIBRARIES})
+target_link_libraries(gztopic gazebo_msgs gazebo_common gazebo_transport pthread gazebo_gui ${QT_LIBRARIES} QtTest)
 gz_install_executable(gztopic)
 
 add_executable(gzlog gzlog.cc)