def appendUrdfModel(self, urdfFilename, frame_name, fMm, prefix=None):
if not isinstance(fMm, pinocchio.SE3):
fMm = pinocchio.XYZQUATToSE3(fMm)
id_parent_frame = self.model.getFrameId(frame_name)
model, gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
geometry_types=pinocchio.GeometryType.COLLISION)
if prefix is not None:
for i in range(1, len(model.names)):
model.names[i] = prefix + model.names[i]
for f in model.frames:
f.name = prefix + f.name
for go in gmodel.geometryObjects:
go.name = prefix + go.name
igeom = self.gmodel.ngeoms
nmodel, ngmodel = pinocchio.appendModel(self.model, model, self.gmodel,
gmodel, id_parent_frame, fMm)
self.gid_field_of_view = ngmodel.getGeometryId("field_of_view")
for go in gmodel.geometryObjects:
ngmodel.addCollisionPair(
pinocchio.CollisionPair(self.gid_field_of_view,
ngmodel.getGeometryId(go.name)))
self.model, self.gmodel = nmodel, ngmodel
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
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 reduceModel(self, jointsToRemove, config, len_prefix=0):
jointIds = [
self.model.getJointId(jn[len_prefix:]) for jn in jointsToRemove
]
self.model, self.gmodel = pinocchio.buildReducedModel(
self.model, self.gmodel, jointIds, np.array(config))
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def __init__(
self,
urdfString=None,
urdfFilename=None,
fov=np.radians((60., 90.)),
# fov = np.radians((49.5, 60)),
geoms=["arm_3_link_0"],
):
if isinstance(fov, str):
self.fov = fov
fov_fcl = hppfcl.MeshLoader().load(
hpp.rostools.retrieve_resource(fov))
else:
# build FOV tetahedron
dx, dy = np.sin(fov)
pts = hppfcl.StdVec_Vec3f()
self.fov = [
(0, 0, 0),
(dx, dy, 1),
(-dx, dy, 1),
(-dx, -dy, 1),
(dx, -dy, 1),
]
pts.extend([np.array(e) for e in self.fov])
self.fov.append(self.fov[1])
fov_fcl = hppfcl.BVHModelOBBRSS()
fov_fcl.beginModel(4, 5)
fov_fcl.addSubModel(pts, _tetahedron_tris)
fov_fcl.endModel()
self.cos_angle_thr = np.cos(np.radians(70))
if urdfFilename is None:
assert urdfString is not None
# Pinocchio does not allow to build a GeometryModel from a XML string.
urdfFilename = '/tmp/tmp.urdf'
with open(urdfFilename, 'w') as f:
f.write(urdfString)
self.model, self.gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
root_joint=pinocchio.JointModelPlanar(),
geometry_types=pinocchio.GeometryType.COLLISION)
id_parent_frame = self.model.getFrameId("xtion_rgb_optical_frame")
parent_frame = self.model.frames[id_parent_frame]
go = pinocchio.GeometryObject("field_of_view", id_parent_frame,
parent_frame.parent, fov_fcl,
parent_frame.placement)
self.gid_field_of_view = self.gmodel.addGeometryObject(go, self.model)
for n in geoms:
assert self.gmodel.existGeometryName(n)
self.gmodel.addCollisionPair(
pinocchio.CollisionPair(self.gmodel.getGeometryId(n),
self.gid_field_of_view))
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def __init__(self, state, actuationModel, costModel):
crocoddyl.DifferentialActionModelAbstract.__init__(self, state, actuationModel.nu, costModel.nr)
self.actuation = actuationModel
self.costs = costModel
self.enable_force = True
self.armature = np.matrix(np.zeros(0))
# We cannot abstract data in Python bindings, let's create this internal data inside model
self.pinocchioData = pinocchio.Data(self.state.pinocchio)
self.actuationData = self.actuation.createData()
self.costsData = self.costs.createData(self.pinocchioData)
self.Minv = None
def loadModelFromString(self, urdfString, rootJointType=pinocchio.JointModelFreeFlyer, removeMimicJoints=True):
""" Load a URDF model contained in a string
- param rootJointType: the root joint type. None for no root joint.
- param removeMimicJoints: if True, all the mimic joints found in the model are removed.
"""
if rootJointType is None:
self.pinocchioModel = pinocchio.buildModelFromXML(urdfString)
else:
self.pinocchioModel = pinocchio.buildModelFromXML(urdfString, rootJointType())
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
if removeMimicJoints:
self._removeMimicJoints(urdfString=urdfString)
def removeJoints(self, joints):
"""
- param joints: a list of joint names to be removed from the self.pinocchioModel
"""
jointIds = list()
for j in joints:
if self.pinocchioModel.existJointName(j):
jointIds.append(self.pinocchioModel.getJointId(j))
if len(jointIds) > 0:
q = pinocchio.neutral(self.pinocchioModel)
self.pinocchioModel = pinocchio.buildReducedModel(self.pinocchioModel, jointIds, q)
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
def createData(self):
data = crocoddyl.DifferentialActionModelAbstract.createData(self)
data.pinocchio = pinocchio.Data(self.state.pinocchio)
data.costs = self.costs.createData(data.pinocchio)
data.shareCostMemory(data.costs)
return data
import consim
from example_robot_data.robots_loader import loadSolo
print("Test solo cpp sim started")
dt = 1e-3
ndt = 10
T = 3.0
kp = 10
kd = 0.05
N = int(T / dt)
# se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ], se3.JointModelFreeFlyer())
robot = loadSolo()
robot.data = se3.Data(robot.model)
if not robot.model.check(robot.data):
print("Python data not consistent with model")
sim = consim.build_simple_simulator(dt, ndt, robot.model, robot.data,
conf.K[0, 0], conf.B[0, 0], conf.K[1, 1],
conf.B[1, 1], conf.mu, conf.mu)
cpts = []
for cf in conf.contact_frames:
if not robot.model.existFrame(cf):
print("ERROR: Frame", cf, "does not exist")
cpts += [sim.add_contact_point(robot.model.getFrameId(cf))]
q = conf.q0
conf.q0[2] += 0.1
dq = zero(robot.nv)
