Merge tag 'v1.2.0'

Release of version 1.2.0.

CMakeLists.txt

@@ -70,10 +70,16 @@ IF(EIGEN_NO_AUTOMATIC_RESIZING)
 ENDIF(EIGEN_NO_AUTOMATIC_RESIZING)
 
 # ----------------------------------------------------
-# --- DEPENDANCIES -----------------------------------
+# --- DEPENDENCIES -----------------------------------
 # ----------------------------------------------------
 ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.2.0") # Eigen::Ref appeared from 3.2.0
-ADD_REQUIRED_DEPENDENCY("pinocchio >= 1.3.0")
+
+# Fail-safe support for catkin-ized pinocchio:
+#  - If catkin-based pinocchio is installed it runs the CFG_EXTRAS to set up the Pinocchio preprocessor directives
+#  - If it isn't, nothing happens and the subsequent pkg-config check takes care of everything.
+#find_package(catkin QUIET COMPONENTS pinocchio)
+
+ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.0.0")
 
 SET(BOOST_REQUIERED_COMPONENTS filesystem system)
 SET(BOOST_BUILD_COMPONENTS unit_test_framework)
@@ -82,7 +88,8 @@ SET(BOOST_OPTIONAL_COMPONENTS "")
 IF(BUILD_PYTHON_INTERFACE)
   SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python)
   FINDPYTHON()
-  INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS}) 
+  INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
+  ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.4.0")
 ENDIF(BUILD_PYTHON_INTERFACE)
 
 SET(BOOST_COMPONENTS ${BOOST_REQUIERED_COMPONENTS} ${BOOST_OPTIONAL_COMPONENTS} ${BOOST_BUILD_COMPONENTS})

bindings/python/CMakeLists.txt

@@ -48,6 +48,7 @@ SET(PYWRAP ${PYWRAP} PARENT_SCOPE)
 
 # --- DEPENDENCIES --- #
 SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 1.4.0")
+SET(PKG_CONFIG_PYWRAP_REQUIRES "pinocchio >= 2.0.0")
 FOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
   ADD_COMPILE_DEPENDENCY(${dep})
 ENDFOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
@@ -110,6 +111,7 @@ ENDIF(BUILD_WITH_COMMIT_VERSION)
 ADD_HEADER_GROUP(${PYWRAP}_HEADERS)
 ADD_SOURCE_GROUP(${PYWRAP}_SOURCES)
 PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} eigenpy)
+PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} pinocchio)
 
 TARGET_LINK_LIBRARIES(${PYWRAP} ${PROJECT_NAME})
 TARGET_LINK_BOOST_PYTHON(${PYWRAP})

bindings/python/contacts/contact-6d.hpp

@@ -81,17 +81,17 @@ namespace tsid
         return name;
       }
 
-      static math::ConstraintEquality computeMotionTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintEquality computeMotionTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.computeMotionTask(t, q, v, data);
         math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
         return cons;
       }
-      static math::ConstraintInequality computeForceTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintInequality computeForceTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.computeForceTask(t, q, v, data);
         math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
         return cons;
       }
-      static math::ConstraintEquality computeForceRegularizationTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintEquality computeForceRegularizationTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.computeForceRegularizationTask(t, q, v, data);
         math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
         return cons;
@@ -127,7 +127,7 @@ namespace tsid
       static bool setMaxNormalForce (Contact6d & self, const double maxNormalForce){
         return self.setMaxNormalForce(maxNormalForce);
       }
-      static void setReference(Contact6d & self, const se3::SE3 & ref){
+      static void setReference(Contact6d & self, const pinocchio::SE3 & ref){
         self.setReference(ref);
       }
       static void setForceReference(Contact6d & self, const::Eigen::VectorXd f_ref){

bindings/python/contacts/contact-point.hpp

@@ -80,17 +80,17 @@ namespace tsid
         return name;
       }
 
-      static math::ConstraintEquality computeMotionTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintEquality computeMotionTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.computeMotionTask(t, q, v, data);
         math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
         return cons;
       }
-      static math::ConstraintInequality computeForceTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintInequality computeForceTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.computeForceTask(t, q, v, data);
         math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
         return cons;
       }
-      static math::ConstraintEquality computeForceRegularizationTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintEquality computeForceRegularizationTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.computeForceRegularizationTask(t, q, v, data);
         math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
         return cons;
@@ -129,7 +129,7 @@ namespace tsid
       static bool setMaxNormalForce (ContactPoint & self, const double maxNormalForce){
         return self.setMaxNormalForce(maxNormalForce);
       }
-      static void setReference(ContactPoint & self, const se3::SE3 & ref){
+      static void setReference(ContactPoint & self, const pinocchio::SE3 & ref){
         self.setReference(ref);
       }
       static void setForceReference(ContactPoint & self, const::Eigen::VectorXd f_ref){

bindings/python/formulations/formulation.hpp

@@ -72,8 +72,8 @@ namespace tsid
         .def("getContactForce", &InvDynPythonVisitor::getContactForce, bp::args("name", "HQPOutput"))
         ;
       }
-      static se3::Data data(const T & self){
-        se3::Data data = self.data();
+      static pinocchio::Data data(const T & self){
+        pinocchio::Data data = self.data();
         return data;
       }
       static bool addMotionTask_SE3(T & self, tasks::TaskSE3Equality & task, double weight, unsigned int priorityLevel, double transition_duration){

bindings/python/robots/robot-wrapper.hpp

@@ -44,7 +44,7 @@ namespace tsid
       {
         cl
         .def(bp::init<std::string, std_vec, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("verbose")), "Default constructor without RootJoint."))
-        .def(bp::init<std::string, std_vec, se3::JointModelVariant, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor without RootJoint."))
+        .def(bp::init<std::string, std_vec, pinocchio::JointModelVariant, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor without RootJoint."))
         .add_property("nq", &Robot::nq)
         .add_property("nv", &Robot::nv)
         
@@ -74,11 +74,11 @@ namespace tsid
    
         ;
       }
-      static se3::Model model (const Robot & self){
+      static pinocchio::Model model (const Robot & self){
         return self.model();
       }
-      static se3::Data data(const Robot & self){
-        se3::Data data(self.model());
+      static pinocchio::Data data(const Robot & self){
+        pinocchio::Data data(self.model());
         return data;
       }
       static Eigen::VectorXd rotor_inertias(const Robot & self){
@@ -94,46 +94,46 @@ namespace tsid
         return self.gear_ratios(gear_ratios);
       }
 
-      static Eigen::Vector3d com (const Robot & self, const se3::Data & data){
+      static Eigen::Vector3d com (const Robot & self, const pinocchio::Data & data){
         return self.com(data);
       }
-      static Eigen::Vector3d com_vel (const Robot & self, const se3::Data & data){
+      static Eigen::Vector3d com_vel (const Robot & self, const pinocchio::Data & data){
         return self.com_vel(data);
       }
-      static Eigen::Vector3d com_acc (const Robot & self, const se3::Data & data){
+      static Eigen::Vector3d com_acc (const Robot & self, const pinocchio::Data & data){
         return self.com_acc(data);
       } 
-      static Matrix3x Jcom (const Robot & self, const se3::Data & data){
+      static Matrix3x Jcom (const Robot & self, const pinocchio::Data & data){
         return self.Jcom(data);
       } 
-      static void computeAllTerms (const Robot & self, se3::Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v){
+      static void computeAllTerms (const Robot & self, pinocchio::Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v){
          self.computeAllTerms(data, q, v);
       }
-      static Eigen::MatrixXd mass (Robot & self, se3::Data & data){
+      static Eigen::MatrixXd mass (Robot & self, pinocchio::Data & data){
         return self.mass(data);
       }
-      static Eigen::VectorXd nonLinearEffects(const Robot & self, const se3::Data & data){
+      static Eigen::VectorXd nonLinearEffects(const Robot & self, const pinocchio::Data & data){
         return self.nonLinearEffects(data);
       }
-      static se3::SE3 position(const Robot & self, const se3::Data & data, const se3::Model::JointIndex & index){
+      static pinocchio::SE3 position(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
         return self.position(data, index);
       }
-      static se3::Motion velocity(const Robot & self, const se3::Data & data, const se3::Model::JointIndex & index){
+      static pinocchio::Motion velocity(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
         return self.velocity(data, index);
       }
-      static se3::Motion acceleration(const Robot & self, const se3::Data & data, const se3::Model::JointIndex & index){
+      static pinocchio::Motion acceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
         return self.acceleration(data, index);
       }
-      static se3::SE3 framePosition(const Robot & self, const se3::Data & data, const se3::Model::FrameIndex & index){
+      static pinocchio::SE3 framePosition(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
         return self.framePosition(data, index);
       }
-      static se3::Motion frameVelocity(const Robot & self, const se3::Data & data, const se3::Model::FrameIndex & index){
+      static pinocchio::Motion frameVelocity(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
         return self.frameVelocity(data, index);
       }
-      static se3::Motion frameAcceleration(const Robot & self, const se3::Data & data, const se3::Model::FrameIndex & index){
+      static pinocchio::Motion frameAcceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
         return self.frameAcceleration(data, index);
       }
-      static se3::Motion frameClassicAcceleration(const Robot & self, const se3::Data & data, const se3::Model::FrameIndex & index){
+      static pinocchio::Motion frameClassicAcceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
         return self.frameClassicAcceleration(data, index);
       }
       

bindings/python/tasks/task-com-equality.hpp

@@ -69,7 +69,7 @@ namespace tsid
         std::string name = self.name();
         return name;
       }
-      static math::ConstraintEquality compute(TaskCOM & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintEquality compute(TaskCOM & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.compute(t, q, v, data);
         math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
         return cons;
@@ -132,4 +132,4 @@ namespace tsid
 }
 
 
-#endif // ifndef __tsid_python_task_com_hpp__
\ No newline at end of file
+#endif // ifndef __tsid_python_task_com_hpp__

bindings/python/tasks/task-joint-posture.hpp

@@ -71,7 +71,7 @@ namespace tsid
         std::string name = self.name();
         return name;
       }
-      static math::ConstraintEquality compute(TaskJoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintEquality compute(TaskJoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.compute(t, q, v, data);
         math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
         return cons;
@@ -140,4 +140,4 @@ namespace tsid
 }
 
 
-#endif // ifndef __tsid_python_task_joint_hpp__
\ No newline at end of file
+#endif // ifndef __tsid_python_task_joint_hpp__

bindings/python/tasks/task-se3-equality.hpp

@@ -70,7 +70,7 @@ namespace tsid
         std::string name = self.name();
         return name;
       }
-      static math::ConstraintEquality compute(TaskSE3 & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
+      static math::ConstraintEquality compute(TaskSE3 & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
         self.compute(t, q, v, data);
         math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
         return cons;
@@ -136,4 +136,4 @@ namespace tsid
 }
 
 
-#endif // ifndef __tsid_python_task_se3_hpp__
\ No newline at end of file
+#endif // ifndef __tsid_python_task_se3_hpp__

bindings/python/trajectories/trajectory-se3.hpp

@@ -41,7 +41,7 @@ namespace tsid
       {
         cl
         .def(bp::init<std::string>((bp::arg("name")), "Default Constructor with name"))
-        .def(bp::init<std::string, se3::SE3>((bp::arg("name"), bp::arg("reference")), "Default Constructor with name and ref_vec"))
+        .def(bp::init<std::string, pinocchio::SE3>((bp::arg("name"), bp::arg("reference")), "Default Constructor with name and ref_vec"))
 
         .add_property("size", &TrajSE3::size)
         .def("setReference", &TrajectorySE3ConstantPythonVisitor::setReference, bp::arg("M_ref"))
@@ -51,7 +51,7 @@ namespace tsid
         .def("getSample", &TrajectorySE3ConstantPythonVisitor::getSample, bp::arg("time"))
         ;
       }
-      static void setReference(TrajSE3 & self, const se3::SE3 & ref){
+      static void setReference(TrajSE3 & self, const pinocchio::SE3 & ref){
           self.setReference(ref);
       }
       static trajectories::TrajectorySample computeNext(TrajSE3 & self){
@@ -81,4 +81,4 @@ namespace tsid
 }
 
 
-#endif // ifndef __tsid_python_traj_se3_hpp__
\ No newline at end of file
+#endif // ifndef __tsid_python_traj_se3_hpp__

cmake @ 1d9aeca2

-Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
+Subproject commit 1d9aeca25e970d2d967fd5be0fb93fe961db121b

demo/demo_quadruped.py

@@ -47,7 +47,7 @@ vector.extend(item for item in path)
 robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
 
 # for gepetto viewer
-robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
+robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
 l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
 if int(l[1]) == 0:
     os.system('gepetto-gui &')

demo/demo_romeo.py

@@ -27,7 +27,7 @@ lf_frame_name = "LAnkleRoll"        # left foot frame name
 contactNormal = np.matrix([0., 0., 1.]).T   # direction of the normal to the contact surface
 w_com = 1.0                     # weight of center of mass task
 w_posture = 1e-3                # weight of joint posture task
-w_forceRef = 1e-5               # weight of force regularization task
+w_forceReg = 1e-5               # weight of force regularization task
 w_RF = 1.0                      # weight of right foot motion task
 kp_contact = 30.0               # proportional gain of contact constraint
 kp_com = 30.0                   # proportional gain of center of mass task
@@ -51,7 +51,7 @@ robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
 srdf = path + '/srdf/romeo_collision.srdf'
 
 # for gepetto viewer .. but Fix me!!
-robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
+robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
 
 l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
 if int(l[1]) == 0:
@@ -61,7 +61,7 @@ cl = gepetto.corbaserver.Client()
 gui = cl.gui
 robot_display.initDisplay(loadModel=True)
 
-q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
+q = se3.getNeutralConfiguration(robot.model(), srdf, False)
 q[2] += 0.84
 v = np.matrix(np.zeros(robot.nv)).transpose()
 
@@ -80,19 +80,19 @@ contact_Point = np.matrix(np.ones((3,4)) * lz)
 contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
 contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
 
-contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
+contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
 contactRF.setKp(kp_contact * np.matrix(np.ones(6)).transpose())
 contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(6)).transpose())
 H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
 contactRF.setReference(H_rf_ref)
-invdyn.addRigidContact(contactRF)
+invdyn.addRigidContact(contactRF, w_forceReg)
 
-contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
+contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
 contactLF.setKp(kp_contact * np.matrix(np.ones(6)).transpose())
 contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(6)).transpose())
 H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
 contactLF.setReference(H_lf_ref)
-invdyn.addRigidContact(contactLF)
+invdyn.addRigidContact(contactLF, w_forceReg)
 
 comTask = tsid.TaskComEquality("task-com", robot)
 comTask.setKp(kp_com * np.matrix(np.ones(3)).transpose())

exercizes/ex_1.py

@@ -52,7 +52,7 @@ robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
 srdf = path + '/srdf/romeo_collision.srdf'
 
 # for gepetto viewer
-robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
+robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
 l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
 if int(l[1]) == 0:
     os.system('gepetto-gui &')
@@ -61,7 +61,7 @@ cl = gepetto.corbaserver.Client()
 gui = cl.gui
 robot_display.initDisplay(loadModel=True)
 
-q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
+q = se3.getNeutralConfiguration(robot.model(), srdf, False)
 q[2] += 0.84
 v = np.matrix(np.zeros(robot.nv)).T
 
@@ -80,19 +80,19 @@ contact_Point = np.matrix(np.ones((3,4)) * lz)
 contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
 contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
 
-contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
+contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
 contactRF.setKp(kp_contact * matlib.ones(6).T)
 contactRF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
 H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
 contactRF.setReference(H_rf_ref)
-invdyn.addRigidContact(contactRF)
+invdyn.addRigidContact(contactRF, w_forceRef)
 
-contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
+contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
 contactLF.setKp(kp_contact * matlib.ones(6).T)
 contactLF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
 H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
 contactLF.setReference(H_lf_ref)
-invdyn.addRigidContact(contactLF)
+invdyn.addRigidContact(contactLF, w_forceRef)
 
 comTask = tsid.TaskComEquality("task-com", robot)
 comTask.setKp(kp_com * matlib.ones(3).T)

exercizes/ex_2.py

@@ -41,7 +41,7 @@ kp_posture = 10.0               # proportional gain of joint posture task
 dt = 0.001                      # controller time step
 PRINT_N = 500                   # print every PRINT_N time steps
 DISPLAY_N = 25                  # update robot configuration in viwewer every DISPLAY_N time steps
-N_SIMULATION = 40000             # number of time steps simulated
+N_SIMULATION = 4000             # number of time steps simulated
 
 filename = str(os.path.dirname(os.path.abspath(__file__)))
 path = filename + '/../models/romeo'
@@ -52,7 +52,7 @@ robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
 srdf = path + '/srdf/romeo_collision.srdf'
 
 # for gepetto viewer
-robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
+robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
 l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
 if int(l[1]) == 0:
     os.system('gepetto-gui &')
@@ -61,7 +61,7 @@ cl = gepetto.corbaserver.Client()
 gui = cl.gui
 robot_display.initDisplay(loadModel=True)
 
-q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
+q = se3.getNeutralConfiguration(robot.model(), srdf, False)
 q[2] += 0.84
 v = np.matrix(np.zeros(robot.nv)).T
 
@@ -80,27 +80,19 @@ contact_Point = np.matrix(np.ones((3,4)) * lz)
 contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
 contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
 
-contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
+contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
 contactRF.setKp(kp_contact * matlib.ones(6).T)
 contactRF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
 H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
 contactRF.setReference(H_rf_ref)
-invdyn.addRigidContact(contactRF)
+invdyn.addRigidContact(contactRF, w_forceRef)
 
-contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
+contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
 contactLF.setKp(kp_contact * matlib.ones(6).T)
 contactLF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
 H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
 contactLF.setReference(H_lf_ref)
-invdyn.addRigidContact(contactLF)
-
-lh_frame_name = 'LWristPitch'
-contactLH =tsid.Contact6d("contact_lhand", robot, lh_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
-contactLH.setKp(kp_contact * matlib.ones(6).T)
-contactLH.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
-H_lh_ref = robot.position(data, robot.model().getJointId(lh_frame_name))
-contactLH.setReference(H_lh_ref)
-invdyn.addRigidContact(contactLH)
+invdyn.addRigidContact(contactLF, w_forceRef)
 
 comTask = tsid.TaskComEquality("task-com", robot)
 comTask.setKp(kp_com * matlib.ones(3).T)
@@ -133,7 +125,7 @@ com_acc_des = matlib.empty((3, N_SIMULATION))*nan # acc_des = acc_ref - Kp*pos_e
 
 offset     = robot.com(data)
 amp        = np.matrix([0.0, 0.05, 0.0]).T
-two_pi_f             = 2*np.pi*np.matrix([0.0, 1.1, 0.0]).T
+two_pi_f             = 2*np.pi*np.matrix([0.0, 0.5, 0.0]).T
 two_pi_f_amp         = np.multiply(two_pi_f,amp)
 two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
 

exercizes/plot_utils.py

+# -*- coding: utf-8 -*-
+"""
+Created on Fri Jan 16 09:16:56 2015
+
+@author: adelpret
+"""
+import matplotlib as mpl
+import matplotlib.pyplot as plt
+import matplotlib.ticker as ticker
+import numpy as np
+
+DEFAULT_FONT_SIZE = 20;
+DEFAULT_AXIS_FONT_SIZE = DEFAULT_FONT_SIZE;
+DEFAULT_LINE_WIDTH = 4; #13;
+DEFAULT_MARKER_SIZE = 6;
+DEFAULT_FONT_FAMILY = 'sans-serif'
+DEFAULT_FONT_SIZE = DEFAULT_FONT_SIZE;
+DEFAULT_FONT_SERIF = ['Times New Roman', 'Times','Bitstream Vera Serif', 'DejaVu Serif', 'New Century Schoolbook', 'Century Schoolbook L', 'Utopia', 'ITC Bookman', 'Bookman', 'Nimbus Roman No9 L', 'Palatino', 'Charter', 'serif'];
+DEFAULT_FIGURE_FACE_COLOR = 'white'    # figure facecolor; 0.75 is scalar gray
+DEFAULT_LEGEND_FONT_SIZE = DEFAULT_FONT_SIZE;
+DEFAULT_AXES_LABEL_SIZE = DEFAULT_FONT_SIZE;  # fontsize of the x any y labels
+DEFAULT_TEXT_USE_TEX = False;
+LINE_ALPHA = 0.9;
+SAVE_FIGURES = False;
+FILE_EXTENSIONS = ['png']; #'pdf']; #['png']; #,'eps'];
+FIGURES_DPI = 150;
+SHOW_LEGENDS = False;
+LEGEND_ALPHA = 0.5;
+SHOW_FIGURES = False;
+FIGURE_PATH = './';
+LINE_WIDTH_RED = 0; # reduction of line width when plotting multiple lines on same plot
+LINE_WIDTH_MIN = 1;
+BOUNDS_COLOR = 'silver';
+
+#legend.framealpha    : 1.0    # opacity of of legend frame
+#axes.hold           : True    # whether to clear the axes by default on
+#axes.linewidth      : 1.0     # edge linewidth
+#axes.titlesize      : large   # fontsize of the axes title
+#axes.color_cycle    : b, g, r, c, m, y, k  # color cycle for plot lines
+#xtick.labelsize      : medium # fontsize of the tick labels
+#figure.dpi       : 80      # figure dots per inch
+#image.cmap   : jet               # gray | jet etc...
+#savefig.dpi         : 100      # figure dots per inch
+#savefig.facecolor   : white    # figure facecolor when saving
+#savefig.edgecolor   : white    # figure edgecolor when saving
+#savefig.format      : png      # png, ps, pdf, svg
+#savefig.jpeg_quality: 95       # when a jpeg is saved, the default quality parameter.
+#savefig.directory   : ~        # default directory in savefig dialog box,
+                                # leave empty to always use current working directory
+
+def create_empty_figure(nRows=1, nCols=1, spinesPos=None,sharex=True):
+    f, ax = plt.subplots(nRows,nCols,sharex=sharex);
+    mngr = plt.get_current_fig_manager()
+#    mngr.window.setGeometry(50,50,1080,720);
+
+    if(spinesPos!=None):
+        if(nRows*nCols>1):
+            for axis in ax.reshape(nRows*nCols):
+                movePlotSpines(axis, spinesPos);
+        else:
+            movePlotSpines(ax, spinesPos);
+    return (f, ax);
+    
+def movePlotSpines(ax, spinesPos):
+    ax.spines['right'].set_color('none')
+    ax.spines['top'].set_color('none')
+    ax.xaxis.set_ticks_position('bottom')
+    ax.spines['bottom'].set_position(('data',spinesPos[0]))
+    ax.yaxis.set_ticks_position('left')
+    ax.spines['left'].set_position(('data',spinesPos[1]))
+    
+def setAxisFontSize(ax, size):
+    for label in ax.get_xticklabels() + ax.get_yticklabels():
+        label.set_fontsize(size)
+        label.set_bbox(dict(facecolor='white', edgecolor='None', alpha=0.65))
+        
+mpl.rcdefaults()
+mpl.rcParams['lines.linewidth']     = DEFAULT_LINE_WIDTH;
+mpl.rcParams['lines.markersize']    = DEFAULT_MARKER_SIZE;
+mpl.rcParams['patch.linewidth']     = 1;
+mpl.rcParams['font.family']         = DEFAULT_FONT_FAMILY;
+mpl.rcParams['font.size']           = DEFAULT_FONT_SIZE;
+mpl.rcParams['font.serif']          = DEFAULT_FONT_SERIF;
+mpl.rcParams['text.usetex']         = DEFAULT_TEXT_USE_TEX;
+mpl.rcParams['axes.labelsize']      = DEFAULT_AXES_LABEL_SIZE;
+mpl.rcParams['legend.fontsize']     = DEFAULT_LEGEND_FONT_SIZE;
+mpl.rcParams['figure.facecolor']    = DEFAULT_FIGURE_FACE_COLOR;
+mpl.rcParams['figure.figsize']      = 23, 12 #12, 9 #
+
+def plot3dQuantity(quantity, title, ax=None, boundUp=None, boundLow=None, yscale='linear', linestyle='k'):
+    return plotNdQuantity(3, 1, quantity, title, ax, boundUp, boundLow, yscale, linestyle);
+    
+def plotNdQuantity(nRows, nCols, quantity, title="", ax=None, boundUp=None, boundLow=None, yscale='linear', 
+                   linestyle='k--', sharey=False, margins=None):
+    t = quantity.shape[0];
+    n = quantity.shape[1];
+    if(margins!=None):
+        if(type(margins) is list):
+            margins = [margins[0].reshape(t,1,n), margins[1].reshape(t,1,n)];
+        else:
+            margins = margins.reshape(t,1,n);
+    return plotNdQuantityPerSolver(nRows, nCols, quantity.reshape(t,1,n), title, None, [linestyle], ax, 
+                                   boundUp, boundLow, yscale, None, None, sharey, margins);
+    
+def plotNdQuantityPerSolver(nRows, nCols, quantity, title, solver_names, line_styles, ax=None, boundUp=None, boundLow=None, 
+                            yscale='linear', subplot_titles=None, ylabels=None, sharey=False, margins=None, x=None):
+    if(ax==None):
+        f, ax = plt.subplots(nRows, nCols, sharex=True, sharey=sharey);
+    ax = ax.reshape(nRows, nCols);
+    k = 0;
+    if(x==None):
+        x = range(quantity.shape[0]);
+    for j in range(nCols):
+        for i in range(nRows):
+            if(k<quantity.shape[2]):
+                if(subplot_titles!=None):
+                    ax[i,j].set_title(subplot_titles[k]);
+                elif(i==0):
+                    ax[i,j].set_title(str(k));  # set titles on first row only