def loadTalos(modelPath='/opt/openrobots/share'):
URDF_FILENAME = "talos_reduced_v2.urdf"
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/talos_data/urdf/" + URDF_FILENAME
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
rmodel = robot.model
pinocchio.getNeutralConfiguration(rmodel, modelPath + SRDF_SUBPATH, False)
pinocchio.loadRotorParameters(rmodel, modelPath + SRDF_SUBPATH, False)
rmodel.armature = \
np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
assert ((rmodel.armature[:6] == 0.).all())
robot.q0.flat[:] = rmodel.neutralConfiguration
"""
robot.q0.flat[:] = [0,0,1.0192720229567027,0,0,0,1,0.0,0.0,-0.411354,0.859395,-0.448041,-0.001708,0.0,0.0,-0.411354,0.859395,-0.448041,-0.001708,0,0.006761,0.25847,0.173046,-0.0002,-0.525366,0,0,0.1,0.5,-0.25847,-0.173046,0.0002,-0.525366,0,0,0.1,0.5,0,0]
"""
return robot
def loadTalosLegs(modelPath='/opt/openrobots/share/example-robot-data'):
from pinocchio import JointModelFreeFlyer, JointModelRX, JointModelRY, JointModelRZ
robot = loadTalos(modelPath=modelPath)
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
legMaxId = 14
m1 = robot.model
m2 = pinocchio.Model()
for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names,
m1.parents, m1.inertias):
if j.id < legMaxId:
jid = m2.addJoint(parent, locals()[j.shortname()](), M, name)
assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
m2.upperPositionLimit = np.matrix([1.] * 19).T
m2.lowerPositionLimit = np.matrix([-1.] * 19).T
#q2 = robot.q0[:19]
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pinocchio.GeometryModel()
for g in robot.visual_model.geometryObjects:
if g.parentJoint < 14:
g2.addGeometryObject(g)
robot.model = m2
robot.data = m2.createData()
robot.visual_model = g2
# robot.q0=q2
robot.visual_data = pinocchio.GeometryData(g2)
# Load SRDF file
pinocchio.getNeutralConfiguration(m2, modelPath + SRDF_SUBPATH, False)
pinocchio.loadRotorParameters(m2, modelPath + SRDF_SUBPATH, False)
m2.armature = \
np.multiply(m2.rotorInertia.flat, np.square(m2.rotorGearRatio.flat))
assert ((m2.armature[:6] == 0.).all())
robot.q0 = m2.neutralConfiguration.copy()
return robot
def __init__(self, conf, viewer=True):
self.conf = conf
vector = se3.StdVec_StdString()
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector, False)
robot = self.robot
self.model = model = robot.model()
try:
q = se3.getNeutralConfiguration(model, conf.srdf, False)
# q = model.referenceConfigurations["half_sitting"]
except:
q = conf.q0
# q = np.matrix(np.zeros(robot.nv)).T
v = np.matrix(np.zeros(robot.nv)).T
assert model.existFrame(conf.ee_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce(
"tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * matlib.ones(robot.nv).T)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) *
matlib.ones(robot.nv).T)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.eeTask = tsid.TaskSE3Equality("task-ee", self.robot,
self.conf.ee_frame_name)
self.eeTask.setKp(self.conf.kp_ee * np.matrix(np.ones(6)).T)
self.eeTask.setKd(2.0 * np.sqrt(self.conf.kp_ee) *
np.matrix(np.ones(6)).T)
self.eeTask.setMask(conf.ee_task_mask)
self.eeTask.useLocalFrame(False)
self.EE = model.getFrameId(conf.ee_frame_name)
H_ee_ref = self.robot.framePosition(formulation.data(), self.EE)
self.trajEE = tsid.TrajectorySE3Constant("traj-ee", H_ee_ref)
formulation.addMotionTask(self.eeTask, conf.w_ee, 1, 0.0)
self.tau_max = conf.tau_max_scaling * model.effortLimit
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds",
robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if (conf.w_torque_bounds > 0.0):
formulation.addActuationTask(actuationBoundsTask,
conf.w_torque_bounds, 0, 0.0)
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot,
conf.dt)
self.v_max = conf.v_max_scaling * model.velocityLimit
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if (conf.w_joint_bounds > 0.0):
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0,
0.0)
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q)
postureTask.setReference(trajPosture.computeNext())
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.trajPosture = trajPosture
self.postureTask = postureTask
self.actuationBoundsTask = actuationBoundsTask
self.jointBoundsTask = jointBoundsTask
self.formulation = formulation
self.solver = solver
self.q = q
self.v = v
# for gepetto viewer
if (viewer):
self.robot_display = se3.RobotWrapper.BuildFromURDF(
conf.urdf, [
conf.path,
])
l = commands.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
self.robot_display.initDisplay(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
self.gui = self.robot_display.viewer.gui
self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
# for gepetto viewer
robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
q = se3.getNeutralConfiguration(robot.model(), srdf, False)
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).T
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
robot_display.display(q)
assert robot.model().existFrame(rf_frame_name)
assert robot.model().existFrame(lf_frame_name)
t = 0.0 # time
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
contact_Point = np.matrix(np.ones((3,4)) * lz)