Merge pull request RobotLocomotion#1711 from RussTedrake/sdf

implements sdf parsing, and lidar sim in c++
YinjieLin · Feb 20, 2016 · 26a9a6e · 26a9a6e
2 parents ae09d81 + 0202056
commit 26a9a6e
Show file tree

Hide file tree

Showing 33 changed files with 5,563 additions and 263 deletions.
diff --git a/drake/examples/Acrobot/Acrobot.h b/drake/examples/Acrobot/Acrobot.h
@@ -0,0 +1,161 @@
+#ifndef _ACROBOT_H_
+#define _ACROBOT_H_
+
+#include <iostream>
+#include <cmath>
+#include "drake/systems/LCMSystem.h"
+
+using namespace std;
+
+template <typename ScalarType = double>
+class AcrobotState  { // models the Drake::Vector concept
+public:
+  typedef drake::lcmt_drake_signal LCMMessageType;
+  static std::string channel() { return "AcrobotState"; };
+
+  AcrobotState(void) : shoulder(0), elbow(0), shoulder_dot(0), elbow_dot(0) {};
+  template <typename Derived>
+  AcrobotState(const Eigen::MatrixBase<Derived>& x) : shoulder(x(0)), elbow(x(1)), shoulder_dot(x(2)), elbow_dot(x(3)) {};
+
+  template <typename Derived>
+  AcrobotState& operator=(const Eigen::MatrixBase<Derived>& x) {
+    shoulder = x(0);
+    elbow = x(1);
+    shoulder_dot = x(2);
+    elbow_dot = x(3);
+    return *this;
+  }
+
+  friend std::string getCoordinateName(const AcrobotState<ScalarType>& vec, unsigned int index) {
+    switch (index) {
+      case 0: return "shoulder";
+      case 1: return "elbow";
+      case 2: return "shoulder_dot";
+      case 3: return "elbow_dot";
+    }
+    return "error";
+  }
+  friend Eigen::Matrix<ScalarType,4,1> toEigen(const AcrobotState<ScalarType>& vec) {
+    Eigen::Matrix<ScalarType,4,1> x;
+    x << vec.shoulder, vec.elbow, vec.shoulder_dot, vec.elbow_dot;
+    return x;
+  };
+
+  const static int RowsAtCompileTime = 4;
+
+  ScalarType shoulder, elbow, shoulder_dot, elbow_dot;
+};
+
+template <typename ScalarType = double>
+class AcrobotInput {
+public:
+  typedef drake::lcmt_drake_signal LCMMessageType;
+  static std::string channel() { return "AcrobotInput"; };
+
+  AcrobotInput(void) : tau(0) {};
+  template <typename Derived>
+  AcrobotInput(const Eigen::MatrixBase<Derived>& x) : tau(x(0)) {};
+
+  template <typename Derived>
+  AcrobotInput& operator=(const Eigen::MatrixBase<Derived>& x) {
+    tau = x(0);
+    return *this;
+  }
+
+  friend std::ostream& operator<<(std::ostream& os, const AcrobotInput& x)
+  {
+    os << "  tau = " << x.tau << endl;
+    return os;
+  }
+
+  const static int RowsAtCompileTime = 1;
+
+  ScalarType tau;
+};
+
+template <typename ScalarType>
+Eigen::Matrix<ScalarType,1,1> toEigen(const AcrobotInput<ScalarType>& vec) {
+  Eigen::Matrix<ScalarType,1,1> x;
+  x << vec.tau;
+  return x;
+};
+
+
+
+class Acrobot {
+public:
+  template <typename ScalarType> using InputVector = AcrobotInput<ScalarType>;
+  template <typename ScalarType> using StateVector = AcrobotState<ScalarType>;
+  template <typename ScalarType> using OutputVector = AcrobotState<ScalarType>;
+
+  Acrobot() :
+          m1(1.0), m2(1.0), // kg
+          l1(1.0), l2(2.0), // m
+          lc1(0.5), lc2(1.0), // m
+          Ic1(.083), Ic2(.33), // kg*m^2
+          b1(0.1), b2(0.1), // kg m^2 /s
+          g(9.81) // m/s^2
+  {}
+  virtual ~Acrobot(void) {};
+
+  template <typename ScalarType>
+  void manipulatorDynamics(const AcrobotState<ScalarType>& x, Eigen::Matrix<ScalarType,2,2>& H, Eigen::Matrix<ScalarType,2,1>& C, Eigen::Matrix<ScalarType,2,1>& B) const
+  {
+    double I1 = Ic1 + m1 * lc1 * lc1;
+    double I2 = Ic2 + m2 * lc2 * lc2;
+    double m2l1lc2 = m2 * l1 * lc2;  // occurs often!
+
+    auto c2 = cos(x.elbow);
+    auto s1 = sin(x.shoulder), s2 = sin(x.elbow);
+    auto s12 = sin(x.shoulder+x.elbow);
+
+    auto h12 = I2 + m2l1lc2 * c2;
+    H << I1 + I2 + m2 * l1 * l1 + 2 * m2l1lc2 * c2, h12, h12, I2;
+//    std::cout << "H = " << H << std::endl;
+
+    C << -2 * m2l1lc2 * s2 * x.elbow_dot * x.shoulder_dot + -m2l1lc2 * s2 * x.elbow_dot*x.elbow_dot, m2l1lc2 * s2 * x.shoulder_dot*x.shoulder_dot;
+
+    // add in G terms
+    C(0) += g * m1 * lc1 * s1 + g * m2 * (l1 * s1 + lc2 * s12);
+    C(1) += g * m2 * lc2 * s12;
+
+    // damping terms
+    C(0) += b1 * x.shoulder_dot;
+    C(1) += b2 * x.elbow_dot;
+//    std::cout << "C = " << C << std::endl;
+
+    // input matrix
+    B << 0.0, 1.0;
+  }
+
+  template <typename ScalarType>
+  AcrobotState<ScalarType> dynamics(const ScalarType& t, const AcrobotState<ScalarType>& x, const AcrobotInput<ScalarType>& u) const {
+    Eigen::Matrix<ScalarType, 2, 2> H;
+    Eigen::Matrix<ScalarType, 2, 1> C;
+    Eigen::Matrix<double, 2, 1> B;  // note: intentionally hard-coding the fact that it's a constant here
+
+    manipulatorDynamics(x,H,C,B);
+    Eigen::Matrix<ScalarType,4,1> xvec = toEigen(x);
+
+    Eigen::Matrix<ScalarType,4,1> xdotvec;
+    xdotvec.topRows(2) = xvec.bottomRows(2);
+    xdotvec.bottomRows(2) = H.inverse()*(B*u.tau - C);
+    return AcrobotState<ScalarType>(xdotvec);
+  }
+
+
+  template <typename ScalarType>
+  AcrobotState<ScalarType> output(const ScalarType& t, const AcrobotState<ScalarType>& x, const AcrobotInput<ScalarType>& u) const {
+    return x;
+  }
+
+  bool isTimeVarying() const  { return false; }
+  bool isDirectFeedthrough() const { return false; }
+
+public:
+  double m1,m2,l1,l2,lc1,lc2,Ic1,Ic2,b1,b2,g;  // parameters (initialized in the constructor)
+};
+
+
+
+#endif // _ACROBOT_H_
diff --git a/drake/examples/Acrobot/Acrobot.sdf b/drake/examples/Acrobot/Acrobot.sdf
@@ -0,0 +1,138 @@
+<sdf version='1.5'>
+  <model name='Acrobot'>
+    <link name='base_link'>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <inertial>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <mass>0</mass>
+        <inertia>
+          <ixx>0</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0</iyy>
+          <iyz>0</iyz>
+          <izz>0</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_link_collision'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.2 0.2 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name='base_link_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.2 0.2 0.2</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+    <link name='upper_link'>
+      <pose frame=''>0 0.15 0 0 -0 0</pose>
+      <inertial>
+        <pose frame=''>0 0 -0.5 0 -0 0</pose>
+        <mass>1</mass>
+        <inertia>
+          <ixx>1</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.083</iyy>
+          <iyz>0</iyz>
+          <izz>1</izz>
+        </inertia>
+      </inertial>
+      <collision name='upper_link_collision'>
+        <pose frame=''>0 0 -0.5 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>1.1</length>
+            <radius>0.05</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+      <visual name='upper_link_visual'>
+        <pose frame=''>0 0 -0.5 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>1.1</length>
+            <radius>0.05</radius>
+          </cylinder>
+        </geometry>
+      </visual>
+    </link>
+    <joint name='shoulder' type='revolute'>
+      <child>upper_link</child>
+      <parent>base_link</parent>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+          <effort>0</effort>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <friction>0</friction>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+    <link name='lower_link'>
+      <pose frame=''>0 0.25 -1 0 -0 0</pose>
+      <inertial>
+        <pose frame=''>0 0 -1 0 -0 0</pose>
+        <mass>1</mass>
+        <inertia>
+          <ixx>1</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.33</iyy>
+          <iyz>0</iyz>
+          <izz>1</izz>
+        </inertia>
+      </inertial>
+      <collision name='lower_link_collision'>
+        <pose frame=''>0 0 -1 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>2.1</length>
+            <radius>0.05</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+      <visual name='lower_link_visual'>
+        <pose frame=''>0 0 -1 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>2.1</length>
+            <radius>0.05</radius>
+          </cylinder>
+        </geometry>
+      </visual>
+    </link>
+    <joint name='elbow' type='revolute'>
+      <child>lower_link</child>
+      <parent>upper_link</parent>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <friction>0</friction>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+  </model>
+</sdf>
diff --git a/drake/examples/Acrobot/CMakeLists.txt b/drake/examples/Acrobot/CMakeLists.txt
@@ -0,0 +1,3 @@
+
+add_subdirectory(@AcrobotPlantCpp)
+add_subdirectory(test)
diff --git a/drake/examples/Acrobot/test/CMakeLists.txt b/drake/examples/Acrobot/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+if (LCM_FOUND)
+  add_executable(acrobotURDFDynamicsTest urdfDynamicsTest.cpp)
+  target_link_libraries(acrobotURDFDynamicsTest drakeRBSystem)
+  add_test(NAME acrobotURDFDynamicsTest COMMAND acrobotURDFDynamicsTest)
+endif(LCM_FOUND)
diff --git a/drake/examples/Acrobot/test/urdfDynamicsTest.cpp b/drake/examples/Acrobot/test/urdfDynamicsTest.cpp
@@ -0,0 +1,50 @@
+
+#include "../Acrobot.h"
+#include "drake/systems/plants/RigidBodySystem.h"
+#include "drake/util/testUtil.h"
+
+using namespace std;
+using namespace Eigen;
+using namespace Drake;
+
+int main(int argc, char* argv[])
+{
+  auto r = Acrobot();
+  auto r_urdf = RigidBodySystem(getDrakePath() + "/examples/Acrobot/Acrobot.urdf", DrakeJoint::FIXED);
+  auto r_sdf = RigidBodySystem(getDrakePath() + "/examples/Acrobot/Acrobot.sdf", DrakeJoint::FIXED);
+
+  // for debugging:
+  /*
+  r_urdf.getRigidBodyTree()->drawKinematicTree("/tmp/urdf.dot");
+  r_sdf.getRigidBodyTree()->drawKinematicTree("/tmp/sdf.dot");
+  */
+  // I ran this at the console to see the output:
+  // dot -Tpng -O /tmp/urdf.dot; dot -Tpng -O /tmp/sdf.dot; open /tmp/*.dot.png
+
+  for (int i=0; i<1000; i++) {
+    auto x0 = getRandomVector<AcrobotState>();
+    auto u0 = getRandomVector<AcrobotInput>();
+
+    RigidBodySystem::StateVector<double> x0_rb = toEigen(x0);
+    RigidBodySystem::InputVector<double> u0_rb = toEigen(u0);
+
+    /*
+    auto kinsol_urdf = r_urdf.getRigidBodyTree()->doKinematics(x0_rb.topRows(2), x0_rb.bottomRows(2));
+    cout << "H_urdf = " << r_urdf.getRigidBodyTree()->massMatrix(kinsol_urdf) << endl;
+    auto kinsol_sdf = r_sdf.getRigidBodyTree()->doKinematics(x0_rb.topRows(2), x0_rb.bottomRows(2));
+    cout << "H_sdf = " << r_sdf.getRigidBodyTree()->massMatrix(kinsol_sdf) << endl;
+    eigen_aligned_unordered_map<const RigidBody *, Matrix<double, 6, 1> > f_ext;
+    cout << "C_urdf = " << r_urdf.getRigidBodyTree()->dynamicsBiasTerm(kinsol_urdf,f_ext) << endl;
+    cout << "C_sdf = " << r_sdf.getRigidBodyTree()->dynamicsBiasTerm(kinsol_sdf,f_ext) << endl;
+    */
+
+    auto xdot = toEigen(r.dynamics(0.0,x0,u0));
+    auto xdot_urdf = r_urdf.dynamics(0.0, x0_rb, u0_rb);
+//    cout << "xdot = " << xdot.transpose() << endl;
+//    cout << "xdot_rb = " << xdot_rb.transpose() << endl;
+    valuecheckMatrix(xdot_urdf,xdot,1e-8);
+
+    auto xdot_sdf = r_sdf.dynamics(0.0, x0_rb, u0_rb);
+    valuecheckMatrix(xdot_sdf,xdot,1e-8);
+  }
+}
diff --git a/drake/examples/CMakeLists.txt b/drake/examples/CMakeLists.txt
@@ -1,8 +1,8 @@
 
 add_subdirectory(Atlas)
 add_subdirectory(Valkyrie)
-add_subdirectory(Acrobot/@AcrobotPlantCpp)
 add_subdirectory(@VanDerPolCpp)
 add_subdirectory(Pendulum)
+add_subdirectory(Acrobot)
 add_subdirectory(Quadrotor)
 add_subdirectory(Cars)
diff --git a/drake/examples/Cars/README.md b/drake/examples/Cars/README.md
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,3 @@

		add_subdirectory(@AcrobotPlantCpp)
		add_subdirectory(test)