Commit cdd5ab7d authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

update for pinocchio 2.4.3

parent 1e95a8ef
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
......@@ -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()
......
......@@ -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 *
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment