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 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 loadIris():
URDF_FILENAME = "iris_simple.urdf"
URDF_SUBPATH = "/iris_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
return robot
def computeWrench(cs, Robot, dt):
rp = RosPack()
urdf = rp.get_path(
Robot.packageName
) + '/urdf/' + Robot.urdfName + Robot.urdfSuffix + '.urdf'
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
model = robot.model
data = robot.data
q_t = cs.concatenateQtrajectories()
dq_t = cs.concatenateQtrajectories()
ddq_t = cs.concatenateQtrajectories()
t = q_t.min()
for phase in cs.contactPhases:
phase.wrench_t = None
t = phase.timeInitial
while t <= phase.timeFinal:
pin.rnea(model, data, phase.q_t(t), phase.dq_t(t), phase.ddq_t(t))
pcom, vcom, acom = robot.com(phase.q_t(t), phase.dq_t(t),
phase.ddq_t(t))
# FIXME : why do I need to call com to have the correct values in data ??
phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
if phase.wrench_t is None:
phase.wrench_t = piecewise(
polynomial(phi0.vector.reshape(-1, 1), t, t))
else:
phase.wrench_t.append(phi0.vector, t)
t += dt
if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.:
t = phase.timeFinal
def __init__(self, name, q, v, fMin, mu, dt, mesh_dir, urdfFileName, freeFlyer=True, detectContactPoint=True):
self.time_step = 0;
self.DETECT_CONTACT_POINTS = detectContactPoint;
self.verb = 0;
self.name = name;
self.mu = mu;
self.fMin = fMin;
self.freeFlyer = freeFlyer;
if(freeFlyer):
self.r = RobotWrapper(urdfFileName, mesh_dir, se3.JointModelFreeFlyer());
else:
self.r = RobotWrapper(urdfFileName, mesh_dir, None);
self.nq = self.r.nq;
self.nv = self.r.nv;
self.na = self.nv-6 if self.freeFlyer else self.nv;
# self.candidateContactConstraints = [constr_rf_fr, constr_rf_fl, constr_rf_hr, constr_rf_hl,
# constr_lf_fr, constr_lf_fl, constr_lf_hr, constr_lf_hl];
self.viewer=Viewer(self.name, self.r);
self.reset(0, q, v, dt);
self.LCP = StaggeredProjections(self.nv, self.mu[0], accuracy=EPS, verb=0);
if(self.DISPLAY_COM):
self.viewer.addSphere('com', self.COM_SPHERE_RADIUS, zeros(3), zeros(3), self.COM_SPHERE_COLOR, 'OFF');
if(self.DISPLAY_CAPTURE_POINT):
self.viewer.addSphere('cp', self.CAPTURE_POINT_SPHERE_RADIUS, zeros(3), zeros(3), self.CAPTURE_POINT_SPHERE_COLOR, 'OFF');
def __init__(self):
urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename)
self.model_path = getModelPath(urdf_path, self.verbose)
self.urdf_path = join(self.model_path, urdf_path)
self.robot = RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')],
pin.JointModelFreeFlyer() if self.free_flyer else None)
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
self.has_rotor_parameters, self.ref_posture)
if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
# Add all collision pairs
self.robot.collision_model.addAllCollisionPairs()
# Remove collision pairs per SRDF
pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)
# Recreate collision data since the collision pairs changed
self.robot.collision_data = self.robot.collision_model.createData()
else:
self.srdf_path = None
self.robot.q0 = pin.neutral(self.robot.model)
if self.free_flyer:
self.addFreeFlyerJointLimits()
def initScenePinocchio(urdf_name,
package_name,
env_name=None,
env_package_name="hpp_environments",
scene_name="world"):
"""
Create and initialize a pinocchio.RobotWrapper with a gepetto-gui display
:param urdf_name: the name of the robot urdf to load
:param package_name: the name of the package containing the robot urdf
:param env_name: the name of the environment file to load
:param env_package_name: the name of the package containing the environment file
:param scene_name: the main node group in the viewer (default to 'world' for pinocchio)
:return: an instance of pinocchio.RobotWrapper and a gepetto.corbaserver.GraphicalInterface
"""
rp = RosPack()
package_path = rp.get_path(package_name)
urdf_path = str(Path(package_path) / 'urdf' / Path(urdf_name + '.urdf'))
robot = pin.RobotWrapper.BuildFromURDF(urdf_path, package_path,
pin.JointModelFreeFlyer())
robot.initDisplay(loadModel=True)
robot.displayCollisions(False)
robot.displayVisuals(True)
#robot.display(robot.model.neutralConfiguration)
cl = gepetto.corbaserver.Client()
gui = cl.gui
if env_name:
urdfEnvPath = rp.get_path(env_package_name)
urdfEnv = urdfEnvPath + '/urdf/' + env_name + '.urdf'
gui.addUrdfObjects(scene_name + "/environments", urdfEnv, True)
return robot, gui
def _build_from_mesh(self, model_wrapper, mesh, bounds=None):
model = model_wrapper.model
geom_model = model_wrapper.geom_model
# save geom_obj placement
geom_obj = mesh.geom_obj()
placement = geom_obj.placement.copy()
geom_obj.placement = pin.SE3.Identity()
geom_obj.q_placement = placement
q_freeflyer = pin.SE3ToXYZQUAT(placement).copy()
# create a freeflyer joint and add it to the model
free_flyer = pin.JointModelFreeFlyer()
universe_joint = 0
joint_name = f"freeflyer_{mesh.name}"
if bounds is not None:
parent_joint = model.addJoint(
universe_joint,
free_flyer,
pin.SE3.Identity(),
joint_name=joint_name,
max_effort=np.array([1000]),
max_velocity=np.array([1000]),
min_config=bounds[0],
max_config=bounds[1],
)
else:
parent_joint = model.addJoint(
universe_joint, free_flyer, pin.SE3.Identity(), joint_name
)
# add geom_obj to the geom_model
geom_obj.parentJoint = parent_joint
geom_model.addGeometryObject(geom_obj)
def loadHector():
URDF_FILENAME = "quadrotor_base.urdf"
URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
return robot
def initialize(self, planner_setting):
package_dirs = [
os.path.dirname(
os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
]
urdf = str(
os.path.dirname(
os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +
'/urdf/quadruped.urdf')
self.robot = RobotWrapper.BuildFromURDF(
urdf,
package_dirs=package_dirs,
root_joint=pin.JointModelFreeFlyer())
self.robot.data = self.robot.model.createData()
self.initDisplay(loadModel=True)
self.robot.viewer.gui.addFloor('world/floor')
self.z_floor = planner_setting.get(PlannerDoubleParam_FloorHeight)
self.robot.viewer.gui.applyConfiguration(
'world/floor', [0.0, 0.0, self.z_floor, 0.0, 0.0, 0.0, 1.0])
self.robot.viewer.gui.refresh()
self.robot.q = self.robot.model.referenceConfigurations.copy()
self.robot.dq = zero(self.robot.model.nv)
self.robot.q[7:] = np.transpose(
np.matrix(
planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)))
pin.forwardKinematics(self.robot.model, self.robot.data, self.robot.q)
self.robot.display(self.robot.q)
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 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 loadANYmal(withArm=None):
if withArm is None:
URDF_FILENAME = "anymal.urdf"
SRDF_FILENAME = "anymal.srdf"
REF_POSTURE = "standing"
elif withArm == "kinova":
URDF_FILENAME = "anymal-kinova.urdf"
SRDF_FILENAME = "anymal-kinova.srdf"
REF_POSTURE = "standing_with_arm_up"
URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model,
modelPath + SRDF_SUBPATH,
False,
False,
referencePose=REF_POSTURE)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot.model)
return robot
def loadRomeo():
URDF_FILENAME = "romeo.urdf"
URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
def buildRobotWrapper(cls):
# Rebuild the robot wrapper instead of using the existing model to
# also load the visuals.
robot = RobotWrapper.BuildFromURDF(cls.urdf_path, cls.meshes_path,
se3.JointModelFreeFlyer())
robot.model.rotorInertia[6:] = cls.motor_inertia
robot.model.rotorGearRatio[6:] = cls.motor_gear_ration
return robot
def main(robotpkg_prefix, robot):
pkg = Path(robotpkg_prefix) / 'share/example-robot-data'
urdf = ROBOTS[robot]
robot = RobotWrapper.BuildFromURDF(str(pkg / urdf), [str(pkg)],
pin.JointModelFreeFlyer())
robot.initDisplay()
robot.loadDisplayModel("pinocchio")
robot.display(robot.q0)
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 __init__(self, nb_legs_pairs, nn_dim, legs_pairs_dist_threshold, shoulders_dist_threshold, urdf = "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo.urdf", modelPath="/opt/openrobots/share/example-robot-data/robots/solo_description/robots/"):
pin.switchToNumpyMatrix()
robot = pin.RobotWrapper.BuildFromURDF( urdf, modelPath, pin.JointModelFreeFlyer())
robot.initViewer(loadModel=True)
if ('viewer' in robot.viz.__dict__):
robot.viewer.gui.setRefreshIsSynchronous(False)
dt = 0.01
self.nbv = NonBlockingViewerFromRobot(robot,dt, nb_pairs=nb_legs_pairs, viz_thresh=3*legs_pairs_dist_threshold, act_thresh_legs=legs_pairs_dist_threshold, act_thresh_shd=shoulders_dist_threshold, shoulder_nn_dim=nn_dim)
def generate_wholebody_load(cfg, cs, fullBody=None, viewer=None):
rp = RosPack()
urdf = rp.get_path(cfg.Robot.packageName) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
logger.info("load robot : %s", urdf)
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
logger.info("robot loaded.")
cs_wb = ContactSequence()
logger.warning("Load wholebody contact sequence from file : %s", cfg.WB_FILENAME)
cs_wb.loadFromBinary(cfg.WB_FILENAME)
return cs_wb, robot
def init(q0):
''' CREATE CONTROLLER AND SIMULATOR '''
#~ global invDynForm
global robot
global na
global simulator
print "reset invdyn"
#~ invDynForm = createInvDynFormUtil(q0, v0);
robot = RobotWrapper( conf.urdfFileName, conf.model_path, root_joint=se3.JointModelFreeFlyer());
simulator = createSimulator(q0, v0);
def loadHyQ(modelPath=None):
URDF_FILENAME = "hyq_no_sensors.urdf"
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
if modelPath is None:
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# TODO define default position inside srdf
robot.q0.flat[7:] = [-0.2, 0.75, -1.5, -0.2, -0.75, 1.5, -0.2, 0.75, -1.5, -0.2, -0.75, 1.5]
robot.q0[2] = 0.57750958
robot.model.referenceConfigurations["half_sitting"] = robot.q0
return robot
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 __init__(
self,
urdf="/opt/openrobots/lib/python3.5/site-packages/../../../share/example-robot-data/robots/solo_description/robots/solo.urdf",
modelPath="/opt/openrobots/lib/python3.5/site-packages/../../../share/example-robot-data/robots",
dt=0.01):
pin.switchToNumpyMatrix()
robot = pin.RobotWrapper.BuildFromURDF(urdf, modelPath,
pin.JointModelFreeFlyer())
robot.initDisplay(loadModel=True)
if ('viewer' in robot.viz.__dict__):
robot.viewer.gui.setRefreshIsSynchronous(False)
self.nbv = NonBlockingViewerFromRobot(robot, dt)
def _config_robot(self, urdf_file, package_dir):
if self._b_fixed_base:
# Fixed based robot
self._model, self._collision_model, self._visual_model = pin.buildModelsFromUrdf(
urdf_file, package_dir)
self._n_floating = 0
else:
# Floating based robot
self._model, self._collision_model, self._visual_model = pin.buildModelsFromUrdf(
urdf_file, package_dir, pin.JointModelFreeFlyer())
self._n_floating = 6
self._data, self._collision_data, self._visual_data = pin.createDatas(
self._model, self._collision_model, self._visual_model)
self._n_q = self._model.nq
self._n_q_dot = self._model.nv
self._n_a = self._n_q_dot - self._n_floating
passing_idx = 0
for j_id, j_name in enumerate(self._model.names):
if j_name == 'root_joint' or j_name == 'universe':
passing_idx += 1
else:
self._joint_id[j_name] = j_id - passing_idx
for f_id, frame in enumerate(self._model.frames):
if frame.name == 'root_joint' or frame.name == 'universe':
pass
else:
if f_id % 2 == 0:
# Link
link_id = int(f_id / 2 - 1)
self._link_id[frame.name] = link_id
else:
# Joint
pass
assert len(self._joint_id) == self._n_a
self._total_mass = sum(
[inertia.mass for inertia in self._model.inertias])
self._joint_pos_limit = np.stack(
[self._model.lowerPositionLimit, self._model.upperPositionLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
self._joint_vel_limit = np.stack(
[-self._model.velocityLimit, self._model.velocityLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
self._joint_trq_limit = np.stack(
[-self._model.effortLimit, self._model.effortLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
def loadHyQ():
URDF_FILENAME = "hyq_no_sensors.urdf"
SRDF_FILENAME = "hyq.srdf"
SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
def loadTalos():
URDF_FILENAME = "talos_reduced.urdf"
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
assert ((robot.model.armature[:6] == 0.).all())
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
def loadHRP(self, urdf, pkgs, loadModel):
'''Internal: load HRP-2 model from URDF.'''
robot = RobotWrapper(urdf, pkgs, root_joint=se3.JointModelFreeFlyer())
robot.initDisplay(loadModel=loadModel)
if 'viewer' not in robot.__dict__: robot.initDisplay()
for n in robot.visual_model.geometryObjects:
robot.viewer.gui.setVisibility(robot.viewerNodeNames(n), 'OFF')
robot.q0 = np.matrix([
0.,
0.,
0.648702,
0.,
0.,
0.,
1., # Free flyer
0.,
0.,
0.,
0., # Chest and head
0.261799,
0.17453,
0.,
-0.523599,
0.,
0.,
0.1, # Left arm
0.261799,
-0.17453,
0.,
-0.523599,
0.,
0.,
0.1, # Right arm
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0., # Left leg
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0., # Righ leg
]).T
self.hrpfull = robot
def __OpenSimJointsToPynocchioJoints(self, PyModel):
jts = 0
for joints in PyModel['Joints']:
dof_in_joint = 6 - (joints[2]['coordinates']).count(None)
if dof_in_joint == 6:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelFreeFlyer()
])
elif dof_in_joint == 3:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelSpherical()
])
elif dof_in_joint == 2:
print '2 dof not supported'
elif dof_in_joint == 1:
for dof in range(0, len(joints[2]['coordinates'])):
if joints[2]['coordinates'][dof] != None:
if joints[2]['name'][dof][0:8] == 'rotation':
if joints[2]['axis'][dof] == ['1', '0', '0']:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelRY()
]) #Y
elif joints[2]['axis'][dof] == ['0', '1', '0']:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelRZ()
]) #Z
elif joints[2]['axis'][dof] == ['0', '0', '1']:
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelRX()
]) #X
else:
v = np.matrix([
np.float64(joints[2]['axis'][dof][0]),
np.float64(joints[2]['axis'][dof][1]),
np.float64(joints[2]['axis'][dof][2])
])
self.joint_models.append([
jts, PyModel['Joints'][jts][0]['name'][0],
se3.JointModelRevoluteUnaligned(
v[0, 2], v[0, 0], v[0, 1])
]) #2,0,1
jts += 1
return self.joint_models
