def loadModelFromUrdf(self,
urdfPath,
urdfDir=None,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True):
"""
Load a model using the pinocchio urdf parser. This parser looks
for urdf files in which kinematics and dynamics information
have been added.
- param urdfPath: a path to the URDF file.
- param urdfDir: A list of directories. If None, will use ROS_PACKAGE_PATH.
"""
if urdfPath.startswith("package://"):
from os import path
n1 = 10 # len("package://")
n2 = urdfPath.index(path.sep, n1)
pkg = urdfPath[n1:n2]
relpath = urdfPath[n2 + 1:]
import rospkg
rospack = rospkg.RosPack()
abspkg = rospack.get_path(pkg)
urdfFile = path.join(abspkg, relpath)
else:
urdfFile = urdfPath
if urdfDir is None:
import os
urdfDir = os.environ["ROS_PACKAGE_PATH"].split(':')
if rootJointType is None:
self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile)
else:
self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile, rootJointType())
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
if removeMimicJoints:
self._removeMimicJoints(urdfFile=urdfFile)
def _build_from_urdf(self, model_wrapper, urdf_path, package_path):
model = model_wrapper.model
geom_model = model_wrapper.geom_model
pin.buildModelFromUrdf(urdf_path, model)
pin.buildGeomFromUrdf(
model, urdf_path, pin.GeometryType.COLLISION, geom_model, package_path,
)
# viz_model = pin.buildGeomFromUrdf(
# model, urdf_path, pin.GeometryType.VISUAL, [packages_path],
# )
# ! keep this line here for body dimensions to be defined (otherwize = -inf)
model_data = model.createData()
geom_model_data = geom_model.createData()
def __init__(self, urdf_path=None):
''' Constructor
'''
# Get parameters from the URDF.
if urdf_path is None:
urdf_path = pkg_resources.resource_filename(
'claptrap_simu', 'data/claptrap.urdf')
model = pnc.buildModelFromUrdf(urdf_path)
self.g = 9.81 # Gravity
self.m = model.inertias[model.getJointId(
"BodyJoint")].mass # Mass of the body
self.mw = model.inertias[model.getJointId(
"WheelJoint")].mass # Mass of the wheel
self.I = model.inertias[model.getJointId("BodyJoint")].inertia[
1, 1] # Body inertia at CoM arong Y
self.J = model.inertias[model.getJointId("WheelJoint")].inertia[
1, 1] # Wheel inertia at CoM around Y
self.l = model.inertias[model.getJointId("BodyJoint")].lever[
2] # Distance of CoM of body from wheel axis
self.r = model.jointPlacements[model.getJointId(
"BodyJoint")].translation[2] # Radius of the wheel
# Define some constants that appear often in the inertia matrix
self.mlr = self.m * self.l * self.r
self.Mr2 = (self.m + self.mw) * self.r**2
# Motor control law
self.motor_control_law = None
def test_self_load(self):
hint_list = [self.model_dir]
model = pin.buildModelFromUrdf(self.model_path,
pin.JointModelFreeFlyer())
collision_model_ref = pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION,
hint_list)
collision_model_self = pin.GeometryModel()
pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION, collision_model_self,
hint_list)
self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
collision_model_self = pin.GeometryModel()
pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION, collision_model_self,
self.model_dir)
self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
hint_vec = pin.StdVec_StdString()
hint_vec.append(self.model_dir)
collision_model_self = pin.GeometryModel()
pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION, collision_model_self,
hint_vec)
self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
def __init__(self):
#Create the object for Abel
abel = AbelMove()
gesture = AbelGesture()
self.model = pinocchio.buildModelFromUrdf(
"/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf"
)
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
numpy_vector_joint_acc = np.random.rand(model.njoints - 1)
# Calls Reverese Newton-Euler algorithm
numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel,
numpy_vector_joint_acc)
computeRNEADerivatives(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel, numpy_vector_joint_acc)
dtorques_dq = data.dtau_dq
dtorques_dqd = data.dtau_dv
dtorques_dqdd = data.M
def test_deprecated_signatures(self):
model = pin.buildModelFromUrdf(self.model_path,
pin.JointModelFreeFlyer())
hint_list = [self.model_dir, "wrong/hint"]
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
hint_list,
pin.GeometryType.COLLISION)
hint_vec = pin.StdVec_StdString()
hint_vec.append(self.model_dir)
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
hint_vec,
pin.GeometryType.COLLISION)
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
self.model_dir,
pin.GeometryType.COLLISION)
if pin.WITH_HPP_FCL_BINDINGS:
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
hint_list,
pin.GeometryType.COLLISION,
pin.hppfcl.MeshLoader())
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
hint_vec,
pin.GeometryType.COLLISION,
pin.hppfcl.MeshLoader())
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
self.model_dir,
pin.GeometryType.COLLISION,
pin.hppfcl.MeshLoader())
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute(
).parents[1].joinpath('urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
# Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, numpy_vector_joint_pos)
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
# Print out the placement of each joint of the kinematic tree
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
def get_manipulator_model():
urdf_filename = pkg_dir + '/data/panda.urdf'
model = pin.buildModelFromUrdf(urdf_filename)
data = model.createData()
print("Created manipulator model: nq={nq}, nv={nv}".format(nq=model.nq,
nv=model.nv))
return Manipulator(model, data)
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
# Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel)
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_linear_vel = joint_velocity.linear
joint_angular_vel = joint_velocity.angular
joint_6v_vel = joint_velocity.vector
err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_linear_vel = frame_velocity.linear
frame_angular_vel = frame_velocity.angular
frame_6v_vel = frame_velocity.vector
err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def test_load(self):
hint_list = [self.model_dir, "wrong/hint"]
expected_mesh_path = os.path.join(self.model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, hint_list)
col = collision_model.geometryObjects[1]
self.assertEqual(col.meshPath, expected_mesh_path)
def __init__(self, urdf_file):
self.urdf_filename = urdf_file
self.model = pin.buildModelFromUrdf(self.urdf_filename,
pin.JointModelFreeFlyer())
self.joint_names = [
"LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll",
"LWristYaw", "LHand"
]
self.joint_ids = [15, 16, 17, 18, 19, 32]
def addPgTaskToUrdfRobot(robot,solver):
# --- ROBOT.PG INIT FRAMES ---
robot.geom = Dynamic("geom")
if(hasattr(robot, 'jointMap')):
for i in robot.jointMap:
robot.geom.addJointMapping(i, robot.jointMap[i])
pinocchioModel = se3.buildModelFromUrdf(robot.urdfFile)
pinocchioData = pinocchioModel.createData()
robot.geom.setModel(pinocchioModel)
robot.geom.setData(pinocchioData)
def addPgTaskToUrdfRobot(robot, solver):
# --- ROBOT.PG INIT FRAMES ---
robot.geom = Dynamic("geom")
if (hasattr(robot, 'jointMap')):
for i in robot.jointMap:
robot.geom.addJointMapping(i, robot.jointMap[i])
pinocchioModel = se3.buildModelFromUrdf(robot.urdfFile)
pinocchioData = pinocchioModel.createData()
robot.geom.setModel(pinocchioModel)
robot.geom.setData(pinocchioData)
def test_multi_load(self):
hint_list = [self.model_dir, "wrong/hint"]
expected_collision_path = os.path.join(
self.model_dir,
'romeo_description/meshes/V1/collision/LHipPitch.dae')
expected_visual_path = os.path.join(
self.model_dir, 'romeo_description/meshes/V1/visual/LHipPitch.dae')
model = pin.buildModelFromUrdf(self.model_path,
pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION,
hint_list)
col = collision_model.geometryObjects[1]
self.assertEqual(col.meshPath, expected_collision_path)
visual_model = pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.VISUAL,
hint_list)
vis = visual_model.geometryObjects[1]
self.assertEqual(vis.meshPath, expected_visual_path)
model_2, collision_model_2, visual_model_2 = pin.buildModelsFromUrdf(
self.model_path, hint_list, pin.JointModelFreeFlyer())
self.assertEqual(model, model_2)
col_2 = collision_model_2.geometryObjects[1]
self.assertEqual(col_2.meshPath, expected_collision_path)
vis_2 = visual_model_2.geometryObjects[1]
self.assertEqual(vis_2.meshPath, expected_visual_path)
model_c, collision_model_c = pin.buildModelsFromUrdf(
self.model_path,
hint_list,
pin.JointModelFreeFlyer(),
geometry_types=pin.GeometryType.COLLISION)
self.assertEqual(model, model_c)
col_c = collision_model_c.geometryObjects[1]
self.assertEqual(col_c.meshPath, expected_collision_path)
model_v, visual_model_v = pin.buildModelsFromUrdf(
self.model_path,
hint_list,
pin.JointModelFreeFlyer(),
geometry_types=pin.GeometryType.VISUAL)
self.assertEqual(model, model_v)
vis_v = visual_model_v.geometryObjects[1]
self.assertEqual(vis_v.meshPath, expected_visual_path)
def __init__(self, name, device=None, tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
print "You ask for ", name, ". "
rospack = RosPack()
self.urdfDir = rospack.get_path('romeo_description') + '/urdf/'
if name == 'romeo_small':
print "Loaded model is romeo_small.urdf."
self.urdfName = 'romeo_small.urdf'
self.halfSitting = self.halfSittingSmall
else:
print "Loaded model is romeo.urdf."
self.urdfName = 'romeo.urdf'
self.halfSitting = self.halfSittingAll
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = {
'left-wrist': 'LWristPitch',
'right-wrist': 'RWristPitch',
'left-ankle': 'LAnkleRoll',
'right-ankle': 'RAnkleRoll',
'gaze': 'gaze_joint',
'waist': 'waist',
'chest': 'TrunkYaw'
}
self.device = device
# correct the name of the body link
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.pinocchioModel = se3.buildModelFromUrdf(
self.urdfDir + self.urdfName, se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
# complete feet position (TODO: move it into srdf file)
#ankle =self.dynamic.getAnklePositionInFootFrame()
#self.ankleLength = 0.1935
#self.ankleWidth = 0.121
#self.dynamic.setFootParameters(True , self.ankleLength, self.ankleWidth, ankle)
#self.dynamic.setFootParameters(False, self.ankleLength, self.ankleWidth, ankle)
# check half sitting size
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError(
"invalid half-sitting pose. HalfSitting Dimension:",
len(self.halfSitting), " .Robot Dimension:", self.dimension)
self.initializeRobot()
def test_load(self):
current_file = os.path.dirname(os.path.abspath(__file__))
model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
model_path = os.path.abspath(os.path.join(model_dir, 'romeo_description/urdf/romeo.urdf'))
expected_mesh_path = os.path.join(model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
hint_list = [model_dir, "wrong/hint"]
model = pin.buildModelFromUrdf(model_path, pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, model_path, pin.utils.fromListToVectorOfString(hint_list), pin.GeometryType.COLLISION)
col = collision_model.geometryObjects[1]
self.assertTrue(col.meshPath == expected_mesh_path)
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
numpy_vector_joint_acc = np.random.rand(model.njoints - 1)
# Calls Reverese Newton-Euler algorithm
numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel,
numpy_vector_joint_acc)
# IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = joint_velocity.vector - \
joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = frame_velocity.vector - \
frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def __init__(self, name,
device = None,
tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
print "You ask for ", name, ". "
rospack = RosPack()
self.urdfDir = rospack.get_path('romeo_description') + '/urdf/'
if name == 'romeo_small':
print "Loaded model is romeo_small.urdf."
self.urdfName = 'romeo_small.urdf'
self.halfSitting = self.halfSittingSmall
else:
print "Loaded model is romeo.urdf."
self.urdfName = 'romeo.urdf'
self.halfSitting = self.halfSittingAll
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = {'left-wrist' : 'LWristPitch',
'right-wrist' : 'RWristPitch',
'left-ankle' : 'LAnkleRoll',
'right-ankle' : 'RAnkleRoll',
'gaze' : 'gaze_joint',
'waist' : 'waist',
'chest' : 'TrunkYaw'}
self.device = device
# correct the name of the body link
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfDir + self.urdfName,
se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
# complete feet position (TODO: move it into srdf file)
#ankle =self.dynamic.getAnklePositionInFootFrame()
#self.ankleLength = 0.1935
#self.ankleWidth = 0.121
#self.dynamic.setFootParameters(True , self.ankleLength, self.ankleWidth, ankle)
#self.dynamic.setFootParameters(False, self.ankleLength, self.ankleWidth, ankle)
# check half sitting size
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose. HalfSitting Dimension:",
len(self.halfSitting), " .Robot Dimension:", self.dimension)
self.initializeRobot()
def test_xml(self):
with open(self.model_path) as model:
file_content = model.read()
model_ref = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
model = pin.buildModelFromXML(file_content,pin.JointModelFreeFlyer())
self.assertEqual(model,model_ref)
model_self = pin.Model()
pin.buildModelFromXML(file_content,pin.JointModelFreeFlyer(),model_self)
self.assertEqual(model_self,model_ref)
def test_load(self):
current_file = os.path.dirname(os.path.abspath(__file__))
model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
model_path = os.path.abspath(os.path.join(model_dir, 'romeo_description/urdf/romeo.urdf'))
expected_mesh_path = os.path.join(model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
hint_list = [model_dir, "wrong/hint"]
model = pin.buildModelFromUrdf(model_path, pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, model_path, pin.utils.fromListToVectorOfString(hint_list), pin.GeometryType.COLLISION)
col = collision_model.geometryObjects[1]
self.assertTrue(col.meshPath == expected_mesh_path)
def create_pinocchio_model():
pinocchio_model_dir = join(dirname(str(abspath(__file__))),
"example_pin_model/")
urdf_filename = pinocchio_model_dir + 'model.urdf'
# Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename)
print('model name: ' + model.name)
# Create data required by the algorithms
data = model.createData()
return model, data
def __init__(self, finger_urdf_path: str,
tip_link_names: typing.Iterable[str]):
"""
Args:
finger_urdf_path: Path to the URDF file describing the robot.
tip_link_names: Names of the finger tip frames, one per finger.
"""
self.robot_model = pinocchio.buildModelFromUrdf(finger_urdf_path)
self.data = self.robot_model.createData()
self.tip_link_ids = [
self.robot_model.getFrameId(link_name)
for link_name in tip_link_names
]
def __init__(self, finger_urdf_path, tip_link_names):
"""
Initializes the finger model on which control's to be performed.
Args:
finger (SimFinger): An instance of the SimFinger class
"""
self.robot_model = pinocchio.buildModelFromUrdf(finger_urdf_path)
self.data = self.robot_model.createData()
self.tip_link_ids = [
self.robot_model.getFrameId(link_name)
for link_name in tip_link_names
]
def __init__(self, env):
self.env = env
self.tri_finger_filename = "trifinger_mod.urdf"
self.model = pin.buildModelFromUrdf(
os.path.join(os.path.dirname(__file__), self.tri_finger_filename))
self.data = self.model.createData()
self.kinematics = TriFingerKin(self.model, self.data, self.env)
self.params = ModelParams()
self.robot_state = None
self.end_effectors_id = [4, 8, 12]
def __init__(self):
"""
Initializes the finger model on which control's to be performed.
"""
self.urdf_path = '/opt/blmc_ei/src/robot_properties_fingers/urdf/pro/trifingerpro.urdf'
self.tip_link_names = [
"finger_tip_link_0",
"finger_tip_link_120",
"finger_tip_link_240",
]
self.robot_model = pinocchio.buildModelFromUrdf(self.urdf_path)
self.data = self.robot_model.createData()
self.tip_link_ids = [
self.robot_model.getFrameId(link_name)
for link_name in self.tip_link_names
]
def __init__(self, name, robotnumber, device=None, tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.device = device
self.AdditionalFrames.append(
("accelerometer", matrixToTuple(self.accelerometerPosition),
"chest"))
self.AdditionalFrames.append(
("gyrometer", matrixToTuple(self.gyrometerPosition), "chest"))
self.AdditionalFrames.append(
("leftFootForceSensor", self.forceSensorInLeftAnkle, "left-ankle"))
self.AdditionalFrames.append(
("rightFootForceSensor", self.forceSensorInRightAnkle,
"right-ankle"))
self.OperationalPointsMap = {
'left-wrist': 'LARM_JOINT5',
'right-wrist': 'RARM_JOINT5',
'left-ankle': 'LLEG_JOINT5',
'right-ankle': 'RLEG_JOINT5',
'gaze': 'HEAD_JOINT1',
'waist': 'WAIST',
'chest': 'CHEST_JOINT1'
}
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
rospack = RosPack()
self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(
robotnumber)) + '/urdf/hrp2_{0}_reduced.urdf'.format(robotnumber)
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath,
se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
if self.dimension != len(self.halfSitting):
raise RuntimeError(
"Dimension of half-sitting: {0} differs from dimension of robot: {1}"
.format(len(self.halfSitting), self.dimension))
self.initializeRobot()
self.dynamic.displayModel()
def __init__(self, name, robotnumber,
device = None, tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.device = device
self.AdditionalFrames.append(
("accelerometer",
matrixToTuple(self.accelerometerPosition), "chest"))
self.AdditionalFrames.append(
("gyrometer",
matrixToTuple(self.gyrometerPosition), "chest"))
self.AdditionalFrames.append(
("leftFootForceSensor",
self.forceSensorInLeftAnkle, "left-ankle"))
self.AdditionalFrames.append(
("rightFootForceSensor",
self.forceSensorInRightAnkle, "right-ankle"))
self.OperationalPointsMap = {'left-wrist' : 'LARM_JOINT5',
'right-wrist' : 'RARM_JOINT5',
'left-ankle' : 'LLEG_JOINT5',
'right-ankle' : 'RLEG_JOINT5',
'gaze' : 'HEAD_JOINT1',
'waist' : 'WAIST',
'chest' : 'CHEST_JOINT1'}
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
rospack = RosPack()
self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(robotnumber)) + '/urdf/hrp2_{0}.urdf'.format(robotnumber)
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
if self.dimension != len(self.halfSitting):
raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dimension))
self.initializeRobot()
self.dynamic.displayModel()
def main():
"""
Main procedure
1 ) Get the path of the urdf model
2 ) Build the pinocchio model
2 ) Print some infor of the model
"""
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
# Load the urdf model
model = buildModelFromUrdf(str(model_file))
model_name = model.name
torque_limits_of_motors = model.effortLimit
velocity_limits_of_motors = model.velocityLimit
number_of_joints = model.njoints
for i in range(number_of_joints):
joint = model.joints[i]
joint_name = model.names[i]
jid = joint.idx_q
joint_placement = model.jointPlacements[i]
numpy_translation_vector = joint_placement.translation
numpy_rotation_matrix = joint_placement.rotation
if jid >= 0:
damping = model.damping[jid]
effortLimit = model.effortLimit[jid]
friction = model.friction[jid]
lowerPositionLimit = model.lowerPositionLimit[jid]
upperPositionLimit = model.upperPositionLimit[jid]
velocityLimit = model.velocityLimit[jid]
number_of_frames = model.nframes
for i in range(number_of_frames):
frame = model.frames[i]
frame_name = frame.name
frame_placement = frame.placement
numpy_translation_vector = frame_placement.translation
numpy_rotation_matrix = frame_placement.rotation
# import pdb
# pdb.set_trace()
for inertia in model.inertias:
print(inertia)
def test_pickle(self):
import pickle
model_dir = os.path.abspath(
os.path.join(self.current_file,
"../../models/example-robot-data/robots"))
model_path = os.path.abspath(
os.path.join(model_dir, "ur_description/urdf/ur5_robot.urdf"))
model = pin.buildModelFromUrdf(model_path)
filename = "model.pickle"
with open(filename, 'wb') as f:
pickle.dump(model, f)
with open(filename, 'rb') as f:
model_copy = pickle.load(f)
self.assertTrue(model == model_copy)
def talker():
rospy.init_node("tocabi_pinocchio", anonymous = False)
rospy.Subscriber("/tocabi/pinocchio/jointstates", JointState, callback)
global model
pub = rospy.Publisher("/tocabi/pinocchio", model, queue_size=1)
modelmsg.CMM = [0 for i in range(33*6)]
modelmsg.COR = [0 for i in range(33*33)]
modelmsg.M = [0 for i in range(33*33)]
modelmsg.g = [0 for i in range(33)]
rate = rospy.Rate(1000) #10hz
model = pinocchio.buildModelFromUrdf("/home/jhk/catkin_ws/src/dyros_tocabi/tocabi_description/robots/dyros_tocabi.urdf",pinocchio.JointModelFreeFlyer())
data = model.createData()
global start
q = pinocchio.randomConfiguration(model)
qdot = pinocchio.utils.zero(model.nv)
qdot_t = pinocchio.utils.zero(model.nv)
qddot_t = pinocchio.utils.zero(model.nv)
while not rospy.is_shutdown():
if start:
global qmsg
for i in range(0, 39):
q[i] = qmsg.position[i]
for j in range(0, 38):
qdot[j] = qmsg.velocity[j]
qdot_t[j] = 0.0
qddot_t[j] = 0.0
CMM = pinocchio.computeCentroidalMap(model, data, q)
pinocchio.crba(model, data, q)
pinocchio.computeCoriolisMatrix(model, data, q, qdot)
pinocchio.rnea(model, data, q, qdot_t, qddot_t)
CMM_slice = CMM[:,6:39]
COR_slice = data.C[:,6:39]
M_slice = data.M[:,6:39]
g_slice = data.tau[6:39]
modelmsg.CMM = CMM_slice.flatten()
modelmsg.COR = COR_slice.flatten()
modelmsg.M = M_slice.flatten()
modelmsg.g = g_slice.flatten()
def check_offset_to_pinocchio_model_in_cart_space(self, q):
import pinocchio
from classic_framework.utils.geometric_transformation import mat2quat
obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf")
model = pinocchio.buildModelFromUrdf(obj_urdf)
data = model.createData()
q_pin = np.zeros(9)
q_pin[:7] = q
pinocchio.framesForwardKinematics(model, data, q_pin)
pinocchio.computeJointJacobians(model, data, q_pin)
pinocchio.framesForwardKinematics(model, data, q_pin)
# get orientation
rotation_matrix = data.oMf[model.getFrameId('panda_grasptarget')].rotation
quaternions = mat2quat(rotation_matrix) # [w x y z]
jac = pinocchio.getFrameJacobian(model, data, model.getFrameId('panda_grasptarget'),
pinocchio.LOCAL_WORLD_ALIGNED)[:, :7]
print('position offset:',
self.current_c_pos - np.array(data.oMf[model.getFrameId('panda_grasptarget')].translation))
def __init__(self, init_j_pos, dt, offset=0, config_path=None):
if config_path is None:
config_path = sim_framework_path('./classic_framework/controllers/Config/PyBullet/FictiveRobot')
super().__init__(config_path, dt=dt)
obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf")
self.model = pinocchio.buildModelFromUrdf(obj_urdf)
self.data = self.model.createData()
# The offset is [0, 0, 0.88] for pybullet (with the table)
# The offset is [0, 0, 0] for mujoco (with current scene)
# NOTE: adjust the offset (z-direction), if you re-place the robot! (Use check offset function of fictive robot)
self.offset = offset
self.init_j_pos = init_j_pos
self.end_effector_frame_id = self.model.getFrameId('panda_grasptarget')
self.des_joint_traj = []
self.gotoJointController = GotoJointController(self.dt)
self.gotoCartPosController = GotoCartPosImpedanceController(self.dt)
self.gotoCartPosQuatController = GotoCartPosQuatImpedanceController(self.dt)
self.jointTrajectoryTracker = JointTrajectoryTracker(self.dt)
import pinocchio as pin from pinocchio.romeo_wrapper import RomeoWrapper import os current_path = os.getcwd() # The model of Romeo is contained in the path PINOCCHIO_GIT_REPOSITORY/models/romeo model_path = current_path + "/" + "../../models/romeo" mesh_dir = model_path urdf_filename = "romeo_small.urdf" urdf_model_path = model_path + "/romeo_description/urdf/" + urdf_filename # Load model model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer()) # Load collision geometries geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,[model_path],pin.GeometryType.COLLISION) # Add collisition pairs geom_model.addAllCollisionPairs() # Remove collision pairs listed in the SRDF file srdf_filename = "romeo_small.srdf" srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename pin.removeCollisionPairs(model,geom_model,srdf_model_path) # Load reference configuration pin.loadReferenceConfigurations(model,srdf_model_path) # Retrieve the half sitting position from the SRDF file
0.0,
0.1,
-0.005, # Right Arm
0.,
0. # Head
]
q = np.matrix(halfSitting).T
print("q:")
print(q.flatten().tolist()[0])
rospack = RosPack()
urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data') + "/../"]
model = pin.buildModelFromUrdf(urdfPath, pin.JointModelFreeFlyer())
data = model.createData()
com = pin.centerOfMass(model, data, q)
pin.updateFramePlacements(model, data)
m = data.mass[0]
h = float(com[2])
g = 9.81
omega = sqrt(g / h)
leftName = param_server_conf.footFrameNames['Left']
leftId = model.getFrameId(leftName)
leftPos = data.oMf[leftId]
rightName = param_server_conf.footFrameNames['Right']
rightId = model.getFrameId(rightName)
rightPos = data.oMf[rightId]
import pinocchio
from sys import argv
filename = "ur5.urdf" if len(argv)<2 else argv[1]
model = pinocchio.buildModelFromUrdf(filename)
data = model.createData()
q = pinocchio.randomConfiguration(model)
print 'q = ', q.T
pinocchio.forwardKinematics(model,data,q)
for k in range(model.njoints):
print("{:<24} : {: .2f} {: .2f} {: .2f}"
.format( model.names[k], *data.oMi[k].translation.T.flat ))
