Merge pull request RobotLocomotion#1788 from rdeits/rd-swig-rigid-body

Expose more IK to python
YinjieLin · Mar 3, 2016 · ab11113 · ab11113
2 parents 24a17a5 + 3bb4e8c
commit ab11113
Show file tree

Hide file tree

Showing 5 changed files with 45 additions and 0 deletions.
diff --git a/drake/bindings/python/.gitignore b/drake/bindings/python/.gitignore
@@ -2,3 +2,4 @@
 pydrake/rbtree.py
 pydrake/autodiffutils.py
 pydrake/solvers/ik.py
+*.pyc
diff --git a/drake/bindings/python/CMakeLists.txt b/drake/bindings/python/CMakeLists.txt
@@ -14,6 +14,7 @@ endfunction(add_python_test)
 
 if (snopt_c_FOUND)
   add_python_test(pydrake.test.testRBTIK)
+  add_python_test(pydrake.test.testPR2IK)
 endif()
 add_python_test(pydrake.test.testRBTTransformPoints)
 add_python_test(pydrake.test.testRBTCoM)
diff --git a/drake/bindings/python/pydrake/test/testPR2IK.py b/drake/bindings/python/pydrake/test/testPR2IK.py
@@ -0,0 +1,24 @@
+import os
+import numpy as np
+import pydrake
+from pydrake.solvers import ik
+
+robot = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/PR2/pr2.urdf"))
+
+# Make sure attribute access works on bodies
+assert robot.bodies[0].linkname == "world"
+
+constraints = [ik.WorldPositionConstraint(robot, 1,
+                                          np.zeros((3,)),
+                                          np.array([0,0,1.0]),
+                                          np.array([0,0,1.0])),
+               ik.WorldPositionConstraint(robot, robot.findLink("r_gripper_palm_link").body_index,
+                                          np.zeros((3,)),
+                                          np.array([0.5,0,1.5]),
+                                          np.array([0.5,0,1.5])),
+               ]
+
+q_seed = robot.getZeroConfiguration()
+options = ik.IKoptions(robot)
+results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints, options)
+print repr(results.q_sol)
diff --git a/drake/bindings/swig/rbtree.i b/drake/bindings/swig/rbtree.i
@@ -15,16 +15,28 @@
 
 %include <typemaps.i>
 %include <std_vector.i>
+
+#define SWIG_SHARED_PTR_NAMESPACE std
+// SWIG has built-in support for shared pointers, and can use either std::shared_ptr
+// or boost::shared_ptr, since they provide similar enough interfaces. Even though
+// the interface file is called 'boost_shared_ptr.i', the above #define tells SWIG
+// to use the std:: implementation instead. Note that this does NOT result in any
+// boost headers being included. 
+%include <boost_shared_ptr.i>
+
 %include <eigen.i>
 %include <autodiff.i>
 %import <autodiffutils.i>
 
 %template(vectorVectorXd) std::vector<Eigen::VectorXd>;
 %template(vectorMatrixXd) std::vector<Eigen::MatrixXd>;
 %template(vectorString) std::vector<std::string>;
+%shared_ptr(RigidBody)
+%template(vectorRigidBody) std::vector<std::shared_ptr<RigidBody> >;
 
 %eigen_typemaps(Eigen::VectorXd)
 %eigen_typemaps(Eigen::Matrix<double, SPACE_DIMENSION, 1>)
+%eigen_typemaps(Eigen::Matrix3Xd)
 %eigen_typemaps(Eigen::Matrix<double, SPACE_DIMENSION, Eigen::Dynamic>)
 %eigen_typemaps(Eigen::MatrixXd)
 %eigen_typemaps(Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>)
@@ -37,6 +49,9 @@
 %template(AutoDiff3XDynamic) AutoDiffWrapper<Eigen::VectorXd, SPACE_DIMENSION, Eigen::Dynamic>;
 %template(AutoDiff3XMax73) AutoDiffWrapper<Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 73>, SPACE_DIMENSION, Eigen::Dynamic>;
 
+%ignore RigidBody::setJoint(std::unique_ptr<DrakeJoint> joint); // unique_ptr confuses SWIG, so we'll ignore it for now
+%include "drake/systems/plants/RigidBody.h"
+
 %immutable RigidBodyTree::actuators;
 %immutable RigidBodyTree::loops;
 %include "drake/systems/plants/RigidBodyTree.h"

diff --git a/drake/systems/plants/RigidBody.h b/drake/systems/plants/RigidBody.h
@@ -124,11 +124,15 @@ class DRAKERBM_EXPORT RigidBody {
     std::shared_ptr<RigidBody> body;
 
    public:
+#ifndef SWIG
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
   };
 
  public:
+#ifndef SWIG
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 
 #endif