Merge pull request #33 from stack-of-tasks/pinocchio_v2

Pinocchio v2, fix #31

Author: nim65s

CMakeLists.txt

@@ -70,9 +70,15 @@ 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
+
+# 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 >= 1.3.0")
 
 SET(BOOST_REQUIERED_COMPONENTS filesystem system)
@@ -83,6 +89,7 @@ IF(BUILD_PYTHON_INTERFACE)
   SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python)
   FINDPYTHON()
   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})

demo/demo_quadruped.py

@@ -47,7 +47,7 @@
 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 @@
 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 @@
 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 @@
 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[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 @@
 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 @@
 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[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 @@
 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 @@
 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 @@
 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[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 @@
 
 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)