update for pinocchio 2.4.3

ospi/model_builder.py

-import pinocchio as se3
 import numpy as np
-import utils
 from IPython import embed
 
+import pinocchio as se3
+import utils
+
+
 class MS:
-   def __init__(self, name="whole-body MS model"):
-      # Pinocchio Model
-      self.model = se3.Model.BuildEmptyModel()
-      # Skelette Meshes
-      self.visuals = []
-      self.visuals.append([0,'ground','none'])
-      self.forces = []
-      self.joint_transformations = []
-      self.name = name
-      #self.FrameType = se3.FrameType.OP_FRAME
-   
-   def buildModel(self, parent, joint_model, joint_placement, joint_name,
-                            joint_id, body_inertia, body_placement, body_name):
-      ''' Add a model to the kinematic three
+    def __init__(self, name="whole-body MS model"):
+        # Pinocchio Model
+        self.model = se3.Model()
+        # Skelette Meshes
+        self.visuals = []
+        self.visuals.append([0, 'ground', 'none'])
+        self.forces = []
+        self.joint_transformations = []
+        self.name = name
+        #self.FrameType = se3.FrameType.OP_FRAME
+
+    def buildModel(self, parent, joint_model, joint_placement, joint_name, joint_id, body_inertia, body_placement,
+                   body_name):
+        ''' Add a model to the kinematic three
       TODO add with bounds, check model.hpp
       '''
-      frame_parent = self.model.getFrameId(joint_name)
-      #print 'frame_parent: ', frame_parent
-      #self.model.addFrame(joint_name, joint_id, frame_parent, joint_placement, se3.FrameType.JOINT)
-      self.model.addJoint(parent, joint_model, joint_placement, joint_name)
-      self.model.addJointFrame(joint_id, frame_parent)
-      ''' Append a body to the given joint in the kinematic tree
+        frame_parent = self.model.getFrameId(joint_name)
+        #print 'frame_parent: ', frame_parent
+        #self.model.addFrame(joint_name, joint_id, frame_parent, joint_placement, se3.FrameType.JOINT)
+        self.model.addJoint(parent, joint_model, joint_placement, joint_name)
+        self.model.addJointFrame(joint_id, frame_parent)
+        ''' Append a body to the given joint in the kinematic tree
       '''
-      self.model.appendBodyToJoint(joint_id, body_inertia, body_placement)
-      self.model.addBodyFrame(body_name, joint_id, body_placement, parent)
-      ''' Add a frame to the frame three i.e. operational points
+        self.model.appendBodyToJoint(joint_id, body_inertia, body_placement)
+        self.model.addBodyFrame(body_name, joint_id, body_placement, parent)
+        ''' Add a frame to the frame three i.e. operational points
       '''
-      #self.model.addFrame(joint_name, parent, idx_f, body_placement, self.FrameType)
-      #return self.model
-      
-   def createVisuals(self, parent, joint_name, filename, scale_factors=None, transform=None):
-      self.visuals.append([parent, joint_name, filename, scale_factors, transform ])
-
-   def createForces(self,force_name,force_type,parent,points):
-      #TODO
-      self.forces.append([force_name,force_type,parent,points])
-
-   def createConstraints(self, qRoM):
-      self.model.lowerPositionLimit = utils.pinocchioCoordinates(self.model, self.joint_transformations, qRoM[:,0])
-      self.model.upperPositionLimit = utils.pinocchioCoordinates(self.model, self.joint_transformations, qRoM[:,1])
-      #TODO
-      #self.Model.effortLimit
-      #self.Model.velocityLimit
-   
-   def createData(self):
-      self.data = self.model.createData()
-      
-   def createJointTransformations(self, joint_transformations):
-      self.joint_transformations = joint_transformations
+        #self.model.addFrame(joint_name, parent, idx_f, body_placement, self.FrameType)
+        #return self.model
+
+    def createVisuals(self, parent, joint_name, filename, scale_factors=None, transform=None):
+        self.visuals.append([parent, joint_name, filename, scale_factors, transform])
+
+    def createForces(self, force_name, force_type, parent, points):
+        #TODO
+        self.forces.append([force_name, force_type, parent, points])
+
+    def createConstraints(self, qRoM):
+        self.model.lowerPositionLimit = utils.pinocchioCoordinates(self.model, self.joint_transformations, qRoM[:, 0])
+        self.model.upperPositionLimit = utils.pinocchioCoordinates(self.model, self.joint_transformations, qRoM[:, 1])
+        #TODO
+        #self.Model.effortLimit
+        #self.Model.velocityLimit
+
+    def createData(self):
+        self.data = self.model.createData()
+
+    def createJointTransformations(self, joint_transformations):
+        self.joint_transformations = joint_transformations

ospi/viewer_utils.py

@@ -3,7 +3,7 @@ import gepetto.corbaserver
 import numpy as np
 import pinocchio as se3
 import os
-from pinocchio.utils import XYZQUATToViewerConfiguration, zero, se3ToXYZQUAT
+from pinocchio.utils import zero, se3ToXYZQUAT
 from pinocchio.utils import zero as mat_zeros
 from IPython import embed
 ENABLE_VIEWER = "ON"
@@ -96,7 +96,6 @@ class Viewer(object):
         layout. If multiple objects have to be placed at the same time, do the refresh only
         at the end of the list
         '''
-        #pinocchioConf = XYZQUATToViewerConfiguration(se3ToXYZQUAT(M))
         pinocchioConf = se3.utils.se3ToXYZQUAT(M)
         self.viewer.gui.applyConfiguration(objName,pinocchioConf)
         if refresh: self.viewer.gui.refresh()

ospi/wrapper.py

@@ -4,7 +4,7 @@ import pinocchio as se3
 import numpy as np
 import time
 import os
-from pinocchio.utils import XYZQUATToViewerConfiguration, zero, se3ToXYZQUAT
+from pinocchio.utils import zero, se3ToXYZQUAT
 from bmtools.algebra import quaternion_from_matrix, euler_matrix
 from bmtools.filters import *