def parseLine(line, i):
# check that line starts with joint_states
if line [0] != 'joint_states':
raise RuntimeError('line {} does not start by keyword "joint_states"'
.format(i))
# look for keyword
ilg = irg = None
# make sure that one and only one of 'left_gripper' and 'right_gripper'
# is specified in the current line.
try:
ilg = line.index('left_gripper')
except ValueError as exc:
pass
try:
irg = line.index('right_gripper')
except ValueError as exc:
pass
if irg is None and ilg is None:
raise SyntaxError \
('line {} contains neither "left_gripper" nor "right_gripper" tag.'
.format(i+1))
if not irg is None and not ilg is None:
raise SyntaxError \
('line {} contains both "left_gripper" and "right_gripper" tags.'
.format(i+1))
measurement = dict()
if ilg:
try:
measurement['joint_states'] = map(float, line [1:ilg])
except ValueError as exc:
raise SyntaxError\
('line {}, tag "joint_states": could not convert list {} to array'.
format(i+1, line [1:ilg]))
try:
v = map(float, line [ilg+1:])
p = Quaternion(x=v[3], y=v[4], z=v[5], w=v[6])
t = np.array(v[0:3]).reshape(3,1)
measurement ['left_gripper'] = SE3(p,t)
except ValueError as exc:
raise SyntaxError\
('line {}, tag "left_gripper": could not convert list {} to array'.
format(i+1, line [ilg+1:]))
if irg:
try:
measurement ['joint_states'] = map(float, line [1:irg])
except ValueError as exc:
raise SyntaxError\
('line {}, tag "joint_states": could not convert list {} to float'.
format(i+1, line [1:irg]))
try:
v = map(float, line [irg+1:])
p = Quaternion(x=v[3], y=v[4], z=v[5], w=v[6])
t = np.array(v[0:3]).reshape(3,1)
measurement ['right_gripper'] = SE3(p,t)
except ValueError as exc:
raise SyntaxError\
('line {}, tag "right_gripper": could not convert list {} to float'.
format(i+1, line [irg+1:]))
return measurement
def createArm3DOF(self,
rootId=0,
prefix='',
jointPlacement=SE3.Identity()):
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
colorred = [1.0, 0.0, 0.0, 1.0]
jointId = rootId
jointName = prefix + "shoulder_joint"
joint = JointModelRX()
jointId = self.model.addJoint(jointId, joint, jointPlacement,
jointName)
self.model.appendBodyToJoint(jointId, Inertia.Random(), SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'shoulder', 0.3,
colorred)
self.visuals.append(
Visual('world/' + prefix + 'shoulder', jointId, SE3.Identity()))
self.viewer.viewer.gui.addBox('world/' + prefix + 'upperarm', .1, .1,
.5, color)
self.visuals.append(
Visual('world/' + prefix + 'upperarm', jointId,
SE3(eye(3), np.array([0., 0., .5]))))
jointName = prefix + "elbow_joint"
jointPlacement = SE3(eye(3), np.array([0, 0, 1.0]))
joint = JointModelRX()
jointId = self.model.addJoint(jointId, joint, jointPlacement,
jointName)
self.model.appendBodyToJoint(jointId, Inertia.Random(), SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'elbow', 0.3,
colorred)
self.visuals.append(
Visual('world/' + prefix + 'elbow', jointId, SE3.Identity()))
self.viewer.viewer.gui.addBox('world/' + prefix + 'lowerarm', .1, .1,
.5, color)
self.visuals.append(
Visual('world/' + prefix + 'lowerarm', jointId,
SE3(eye(3), np.array([0., 0., .5]))))
jointName = prefix + "wrist_joint"
jointPlacement = SE3(eye(3), np.array([0, 0, 1.0]))
joint = JointModelRX()
jointId = self.model.addJoint(jointId, joint, jointPlacement,
jointName)
self.model.appendBodyToJoint(jointId, Inertia.Random(), SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'wrist', 0.3,
colorred)
self.visuals.append(
Visual('world/' + prefix + 'wrist', jointId, SE3.Identity()))
self.viewer.viewer.gui.addBox('world/' + prefix + 'hand', .1, .1, .25,
color)
self.visuals.append(
Visual('world/' + prefix + 'hand', jointId,
SE3(eye(3), np.array([0., 0., .25]))))
def __init__(self, debug: bool = False, **kwargs):
# Get the urdf and mesh paths
data_root_dir = resource_filename("gym_jiminy.envs",
"data/bipedal_robots/cassie")
urdf_path = os.path.join(data_root_dir, "cassie.urdf")
# Initialize the walker environment
super().__init__(
**{
**dict(urdf_path=urdf_path,
mesh_path=data_root_dir,
simu_duration_max=SIMULATION_DURATION,
step_dt=STEP_DT,
reward_mixture=REWARD_MIXTURE,
std_ratio=STD_RATIO,
avoid_instable_collisions=False,
debug=debug),
**kwargs
})
# Add missing pushrod close kinematic chain constraint
M_pushrod_tarsus_right = SE3(np.eye(3), np.array([-0.12, 0.03,
-0.005]))
M_pushrod_hip_right = SE3(np.eye(3), np.array([0.0, 0.0, -0.045]))
self.robot.add_frame("right_pushrod_tarsus", "right_tarsus",
M_pushrod_tarsus_right)
self.robot.add_frame("right_pushrod_hip", "hip_flexion_right",
M_pushrod_hip_right)
pushrod_right = DistanceConstraint("right_pushrod_tarsus",
"right_pushrod_hip", 0.5012)
self.robot.add_constraint("pushrod_right", pushrod_right)
M_pushrod_tarsus_left = SE3(np.eye(3), np.array([-0.12, 0.03, 0.005]))
M_pushrod_hip_left = SE3(np.eye(3), np.array([0.0, 0.0, 0.045]))
self.robot.add_frame("left_pushrod_tarsus", "left_tarsus",
M_pushrod_tarsus_left)
self.robot.add_frame("left_pushrod_hip", "hip_flexion_left",
M_pushrod_hip_left)
pushrod_left = DistanceConstraint("left_pushrod_tarsus",
"left_pushrod_hip", 0.5012)
self.robot.add_constraint("pushrod_left", pushrod_left)
# Replace knee to shin spring by fixed joint constraint
right_spring_knee_to_shin = JointConstraint("knee_to_shin_right")
self.robot.add_constraint("right_spring_knee_to_shin",
right_spring_knee_to_shin)
left_spring_knee_to_shin = JointConstraint("knee_to_shin_left")
self.robot.add_constraint("left_spring_knee_to_shin",
left_spring_knee_to_shin)
def setCameraTransform(self,
translation=np.zeros(3),
rotation=np.zeros(3),
relative=False):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
H_abs = SE3(R_pnc, np.array(translation))
if relative:
H_orig = XYZQUATToSe3(
self._client.getCameraTransform(self._window_id))
H_abs = H_abs * H_orig
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(H_abs).tolist())
else:
if relative:
raise RuntimeError(
"'relative'=True not available with meshcat.")
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def transQuatToSE3(p):
from pinocchio import SE3, Quaternion
from numpy import matrix
if len(p) != 7:
raise ValueError("Cannot convert {} to SE3".format(p))
return SE3(
Quaternion(p[6], p[3], p[4], p[5]).matrix(),
matrix(p[0:3]).transpose())
def __call__(self, T, nu):
"""
Integrate se3 velocity from SE3 element T
- T: instance of SE3
- nu: numpy array of dimension 6 (v,omega)
"""
q0 = np.array(7*[0.])
q0[0:3] = T.translation
q0[3:7] = Quaternion(T.rotation).coeffs()
q = integrate(self.model,q0,nu)
return SE3(Quaternion(x=q[3], y=q[4], z=q[5], w=q[6]), q[0:3])
def jacobian(self, q):
self.robot.mass(q)
com_p = self.robot.com(q)
cXi= SE3.Identity()
oXi = self.robot.data.oMi[1]
cXi.rotation = oXi.rotation
cXi.translation = oXi.translation - com_p
M_ff = self.robot.data.M[:6,:]
M_com = cXi.inverse().np.T * M_ff
L_dot = M_com[3:,:]
wXc = SE3(eye(3),self.robot.position(q,1).inverse()*self.robot.com(q))
Jang = wXc.action.T[3:,:]*self.robot.mass(q)[:6,:]
return self._coeff * L_dot[self._mask,:]
def setCameraTransform(self, translation, rotation):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
T_pnc = np.array(translation)
T_R = SE3(R_pnc, T_pnc)
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(T_R).tolist())
else:
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def testJacobian(self):
# Derivatives with respect to q_off
nj = len(self.joints)
dq = 1e-6
self.computeValueAndJacobian()
J = self.jacobian.copy()
q_off0 = self.variable.q_off.copy()
for i in range(nj):
self.variable.q_off[i] = q_off0[i]-dq
self.computeValueAndJacobian()
v0 = self.value.copy()
self.variable.q_off[i] = q_off0[i]+dq
self.computeValueAndJacobian()
v1 = self.value.copy()
error = (v1 - v0)/(2*dq) - J[:,i:i+1]
print ("||error|| = {}".format(norm(error)))
self.variable.q_off = q_off0
# Derivatives wrt hTc
hTc0 = SE3(self.variable.hTc)
for i in range(6):
nu = np.zeros((6,1))
nu[i] = -dq
self.variable.hTc = self.integrate(hTc0, nu)
self.computeValueAndJacobian()
v0 = self.value.copy()
nu[i] = dq
self.variable.hTc = self.integrate(hTc0, nu)
self.computeValueAndJacobian()
v1 = self.value.copy()
error = (v1 - v0)/(2*dq) - J[:,nj+i:nj+i+1]
print ("||error|| = {}".format(norm(error)))
self.variable.hTc = hTc0
# Derivatives with respect to lwTls
lwTls0 = SE3(self.variable.lwTls)
for i in range(6):
nu = np.zeros((6,1))
nu[i] = -dq
self.variable.lwTls = self.integrate(lwTls0, nu)
self.computeValueAndJacobian()
v0 = self.value.copy()
nu[i] = dq
self.variable.lwTls = self.integrate(lwTls0, nu)
self.computeValueAndJacobian()
v1 = self.value.copy()
error = (v1 - v0)/(2*dq) - J[:,nj+6+i:nj+6+i+1]
print ("||error|| = {}".format(norm(error)))
self.variable.lwTls = lwTls0
# Derivatives with respect to rwTrs
rwTrs0 = SE3(self.variable.rwTrs)
for i in range(6):
nu = np.zeros((6,1))
nu[i] = -dq
self.variable.rwTrs = self.integrate(rwTrs0, nu)
self.computeValueAndJacobian()
v0 = self.value.copy()
nu[i] = dq
self.variable.rwTrs = self.integrate(rwTrs0, nu)
self.computeValueAndJacobian()
v1 = self.value.copy()
error = (v1 - v0)/(2*dq) - J[:,nj+12+i:nj+12+i+1]
print ("||error|| = {}".format(norm(error)))
self.variable.rwTrs = rwTrs0
print('===CBK=== {0:4d} {1: 3.2f} {2: 3.2f}'.format(
self.nfeval, q[0], q[1]))
q = np.matrix(q).T
robot.display(q)
# place('world/blue', robot.placement(q, robot.rh))
self.nfeval += 1
time.sleep(self.dt)
# Question 1-2-3
rh = np.matrix([1.5, 0.1, 1.5]).T # x,y,z
rf = np.matrix([0., -0.1, 0.0]).T # x,y,z
lf = np.matrix([0., 0.1, 0.0]).T # x,y,z
com = np.matrix([0.0, 0.0, 0.5]).T # x,y,z
place('world/red', SE3(eye(3), rh))
place('world/blue', SE3(eye(3), rf))
place('world/green', SE3(eye(3), lf))
place('world/yellow', SE3(eye(3), com))
cost = Cost(rh, rf, lf, com)
# If the quaternion is not unitary, it does not correspond a valid rotation. The
# behavior is then not mathematically valid and is implementation dependant. In
# our implementation, quaternions with norm larger than one are interpreted as
# rotation+scaling.
print('Optimize with any (non unitary) quaternion')
cost.weightQuat = 0.0
qopt = fmin_bfgs(cost, robot.q0, callback=CallbackLogger(.1))
# We implement the constraint as a cost with high weight. The quaternion is now unitary.
def build_cs_from_sl1m_l1(fb, q_ref, root_end, pb, RF, allfeetpos, use_orientation, use_interpolated_orientation,
q_init = None, first_phase = None):
"""
Build a multicontact_api.ContactSequence from the SL1M outputs, when not using the MIP formulation
:param fb: an instance of rbprm.Fullbody
:param q_ref: the reference wholebody configuration of the robot
:param root_end: the final base position
:param pb: the SL1M problem dictionary, containing all the contact surfaces and data
:param RF: the Id of the right feet in the SL1M formulation
:param allfeetpos: the list of all foot position for each phase, computed by SL1M
:param use_orientation: if True, change the contact yaw rotation to match the orientation of the base in the guide
:param use_interpolated_orientation: if True, the feet yaw orientation will 'anticipate' the base orientation
of the next phase
:param q_init: the initial wholebody configuration (either this or first_phase should be provided)
:param first_phase: the first multicontact_api.ContactPhase object (either this or q_init should be provided)
:return: the multicontact_api.ContactSequence, with all ContactPhase created at the correct placement
"""
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequence(0)
if q_init:
addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId])
elif first_phase:
cs.append(first_phase)
else:
raise ValueError("build_cs_from_sl1m should have either q_init or first_phase argument defined")
# loop over all phases of pb and add them to the cs :
for pId in range(2, len(pb["phaseData"])): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contactPhases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += EPS_Z # FIXME it shouldn't be required !!
# compute desired foot rotation :
if use_orientation:
rot = compute_orientation_for_feet_placement(fb, pb, pId, moving, RF, prev_contactPhase, use_interpolated_orientation)
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.array(pos).T
placement.rotation = rot.matrix()
cs.moveEffectorToPlacement(movingID, placement)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = q_ref.tolist() + [0] * 6
#p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
#for i in range(3):
# q_end[i] += p_end[i]
q_end[0:7] = root_end
feet_height_end = allfeetpos[-1][2]
logger.info("feet height final = %s", feet_height_end)
q_end[2] = feet_height_end + q_ref[2]
q_end[2] += EPS_Z
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
return cs
def generateContactSequence():
#RF,root_init,pb, coms, footpos, allfeetpos, res = runLPScript()
RF, root_init, root_end, pb, coms, footpos, allfeetpos, res = runLPFromGuideScript(
)
multicontact_api.switchToNumpyArray()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, True)
q_init = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
q_init[0:7] = root_init
feet_height_init = allfeetpos[0][2]
print("feet height initial = ", feet_height_init)
q_init[2] = feet_height_init + cfg.IK_REFERENCE_CONFIG[2]
q_init[2] += EPS_Z
#q_init[2] = fb.referenceConfig[2] # 0.98 is in the _path script
if v:
v(q_init)
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequence(0)
addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId])
# loop over all phases of pb and add them to the cs :
for pId in range(
2, len(pb["phaseData"])
): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contactPhases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += EPS_Z # FIXME it shouldn't be required !!
# compute desired foot rotation :
if cfg.SL1M_USE_ORIENTATION:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if pId < len(pb["phaseData"]) - 1:
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
else:
quat1 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if cfg.SL1M_USE_INTERPOLATED_ORIENTATION:
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(
prev_contactPhase.contactPatch(
fb.lfoot).placement.rotation)
else:
ql = rot
qr = Quaternion(
prev_contactPhase.contactPatch(
fb.rfoot).placement.rotation)
rpy = matrixToRpy((qr * (ql.inverse())).matrix(
)) # rotation from the left foot pose to the right one
if rpy[2, 0] > 0: # yaw positive, feet are crossing
rot = quat0 # rotation of the root, from the guide
else:
rot = quat0 # rotation of the root, from the guide
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.array(pos).T
placement.rotation = rot.matrix()
cs.moveEffectorToPlacement(movingID, placement)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
#p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
#for i in range(3):
# q_end[i] += p_end[i]
q_end[0:7] = root_end
feet_height_end = allfeetpos[-1][2]
print("feet height final = ", feet_height_end)
q_end[2] = feet_height_end + cfg.IK_REFERENCE_CONFIG[2]
q_end[2] += EPS_Z
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
if cfg.DISPLAY_CS_STONES:
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v
def get_transform(origin):
from pinocchio import SE3, rpy
_xyz = [float(v) for v in origin.attrib.get('xyz', '0 0 0').split(' ')]
_rpy = [float(v) for v in origin.attrib.get('rpy', '0 0 0').split(' ')]
return SE3(rpy.rpyToMatrix(*_rpy), np.array(_xyz))
def __init__(self):
self.model = Model()
self.model.addJoint(0, JointModelFreeFlyer(), SE3(), "SE3")
def XYZRPYToSe3(xyzrpy):
return SE3(rpyToMatrix(xyzrpy[3:]), xyzrpy[:3])
def createMultiphaseShootingProblem(rmodel, rdata, csw, timeStep):
"""
Create a Multiphase Shooting problem from the output of the centroidal solver.
:params rmodel: robot model of type pinocchio::model
:params rdata: robot data of type pinocchio::data
:params csw: contact sequence wrapper of type ContactSequenceWrapper
:params timeStep: Scalar timestep between nodes.
:returns list of IntegratedActionModels
"""
# -----------------------
# Define Cost weights
class Weights:
com = 1e1
regx = 1e-3
regu = 0.
swing_patch = 1e6
forces = 0.
contactv = 1e3
# Define state cost vector for WeightedActivation
ff_orientation = 1e1
xweight = np.array([0] * 3 + [ff_orientation] * 3 + [1.] * (rmodel.nv - 6) + [1.] * rmodel.nv)
xweight[range(18, 20)] = ff_orientation
# for patch in swing_patch: w.swing_patch.append(100.);
# Define weights for the impact costs.
imp_state = 1e2
imp_com = 1e2
imp_contact_patch = 1e6
imp_act_com = m2a([0.1, 0.1, 3.0])
# Define weights for the terminal costs
term_com = 1e8
term_regx = 1e4
w = Weights()
# ------------------------
problem_models = []
actuationff = ActuationModelFreeFloating(rmodel)
State = StatePinocchio(rmodel)
active_contact_patch = set()
active_contact_patch_prev = set()
for nphase, phase in enumerate(csw.cs.contact_phases):
t0 = phase.time_trajectory[0]
t1 = phase.time_trajectory[-1]
N = int(round((t1 - t0) / timeStep)) + 1
contact_model = ContactModelMultiple(rmodel)
# Add contact constraints for the active contact patches.
# Add SE3 cost for the non-active contact patches.
swing_patch = []
active_contact_patch_prev = active_contact_patch.copy()
active_contact_patch.clear()
for patch in csw.ee_map.keys():
if getattr(phase, patch).active:
active_contact_patch.add(patch)
active_contact = ContactModel6D(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=getattr(phase, patch).placement)
contact_model.addContact(patch, active_contact)
# print nphase, "Contact ",patch," added at ", getattr(phase,patch).placement.translation.T
else:
swing_patch.append(patch)
# Check if contact has been added in this phase. If this phase is not zero,
# add an impulse model to deal with this contact.
new_contacts = active_contact_patch.difference(active_contact_patch_prev)
if nphase != 0 and len(new_contacts) != 0:
# print nphase, "Impact ",[p for p in new_contacts]," added"
imp_model = ImpulseModelMultiple(
rmodel, {
"Impulse_" + patch: ImpulseModel6D(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]))
for patch in new_contacts
})
# Costs for the impulse of a new contact
cost_model = CostModelSum(rmodel, nu=0)
# State
cost_regx = CostModelState(rmodel,
State,
ref=rmodel.defaultState,
nu=actuationff.nu,
activation=ActivationModelWeightedQuad(w.xweight))
cost_model.addCost("imp_regx", cost_regx, w.imp_state)
# CoM
cost_com = CostModelImpactCoM(rmodel, activation=ActivationModelWeightedQuad(w.imp_act_com))
cost_model.addCost("imp_CoM", cost_com, w.imp_com)
# Contact Frameplacement
for patch in new_contacts:
cost_contact = CostModelFramePlacement(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=SE3(np.identity(3), csw.ee_splines[patch].eval(t0)[0]),
nu=actuationff.nu)
cost_model.addCost("imp_contact_" + patch, cost_contact, w.imp_contact_patch)
imp_action_model = ActionModelImpact(rmodel, imp_model, cost_model)
problem_models.append(imp_action_model)
# Define the cost and action models for each timestep in the contact phase.
# untill [:-1] because in contact sequence timetrajectory, the end-time is
# also included. e.g., instead of being [0.,0.5], time trajectory is [0,0.5,1.]
for t in np.linspace(t0, t1, N)[:-1]:
cost_model = CostModelSum(rmodel, actuationff.nu)
# For the first node of the phase, add cost v=0 for the contacting foot.
if t == 0:
for patch in active_contact_patch:
cost_vcontact = CostModelFrameVelocity(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=m2a(Motion.Zero().vector),
nu=actuationff.nu)
cost_model.addCost("contactv_" + patch, cost_vcontact, w.contactv)
# CoM Cost
cost_com = CostModelCoM(rmodel, ref=m2a(csw.phi_c.com_vcom.eval(t)[0][:3, :]), nu=actuationff.nu)
cost_model.addCost("CoM", cost_com, w.com)
# Forces Cost
for patch in contact_model.contacts.keys():
cost_force = CostModelForce(rmodel,
contactModel=contact_model.contacts[patch],
ref=m2a(csw.phi_c.forces[patch].eval(t)[0]),
nu=actuationff.nu)
cost_model.addCost("forces_" + patch, cost_force, w.forces)
# Swing patch cost
for patch in swing_patch:
cost_swing = CostModelFramePlacement(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=SE3(np.identity(3), csw.ee_splines[patch].eval(t)[0]),
nu=actuationff.nu)
cost_model.addCost("swing_" + patch, cost_swing, w.swing_patch)
# print t, "Swing cost ",patch," added at ", csw.ee_splines[patch].eval(t)[0][:3].T
# State Regularization
cost_regx = CostModelState(rmodel,
State,
ref=rmodel.defaultState,
nu=actuationff.nu,
activation=ActivationModelWeightedQuad(w.xweight))
cost_model.addCost("regx", cost_regx, w.regx)
# Control Regularization
cost_regu = CostModelControl(rmodel, nu=actuationff.nu)
cost_model.addCost("regu", cost_regu, w.regu)
dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff, contact_model, cost_model)
imodel = IntegratedActionModelEuler(dmodel, timeStep=timeStep)
problem_models.append(imodel)
# Create Terminal Model.
contact_model = ContactModelMultiple(rmodel)
# Add contact constraints for the active contact patches.
swing_patch = []
t = t1
for patch in csw.ee_map.keys():
if getattr(phase, patch).active:
active_contact = ContactModel6D(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=getattr(phase, patch).placement)
contact_model.addContact(patch, active_contact)
cost_model = CostModelSum(rmodel, actuationff.nu)
# CoM Cost
cost_com = CostModelCoM(rmodel, ref=m2a(csw.phi_c.com_vcom.eval(t)[0][:3, :]), nu=actuationff.nu)
cost_model.addCost("CoM", cost_com, w.term_com)
# State Regularization
cost_regx = CostModelState(rmodel,
State,
ref=rmodel.defaultState,
nu=actuationff.nu,
activation=ActivationModelWeightedQuad(w.xweight))
cost_model.addCost("regx", cost_regx, w.term_regx)
dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff, contact_model, cost_model)
imodel = IntegratedActionModelEuler(dmodel)
problem_models.append(imodel)
problem_models.append
return problem_models
def contactSequenceFromRBPRMConfigs(fb, configs, beginId, endId):
print "generate contact sequence from planning : "
global i_sphere
i_sphere = 0
#print "MR offset",MRsole_offset
#print "ML offset",MLsole_offset
n_double_support = len(configs)
# config only contains the double support stance
n_steps = n_double_support * 2 - 1
# Notice : what we call double support / simple support are in fact the state with all the contacts and the state without the next moving contact
cs = ContactSequenceHumanoid(n_steps)
unusedPatch = cs.contact_phases[0].LF_patch.copy()
unusedPatch.placement = SE3.Identity()
unusedPatch.active = False
# for each contact state we must create 2 phase (one with all the contact and one with the next replaced contact(s) broken)
for stateId in range(beginId, endId + 1):
# %%%%%%%%% all the contacts : %%%%%%%%%%%%%
cs_id = (stateId - beginId) * 2
config_id = stateId - beginId
phase_d = cs.contact_phases[cs_id]
fb.setCurrentConfig(configs[config_id])
"""
init_guess_for_phase = init_guess_provided
if init_guess_for_phase:
c_init_guess = curves_initGuess[config_id]
t_init_guess = timings_initGuess[config_id]
init_guess_for_phase = isinstance(c_init_guess,bezier)
if init_guess_for_phase:
print "bezier curve provided for config id : "+str(config_id)
"""
# compute MRF and MLF : the position of the contacts
q_rl = fb.getJointPosition(fb.rfoot)
q_ll = fb.getJointPosition(fb.lfoot)
q_rh = fb.getJointPosition(fb.rhand)
q_lh = fb.getJointPosition(fb.lhand)
# feets
MRF = SE3.Identity()
MLF = SE3.Identity()
MRF.translation = np.matrix(q_rl[0:3]).T
MLF.translation = np.matrix(q_ll[0:3]).T
if not cfg.FORCE_STRAIGHT_LINE:
rot_rl = quatFromConfig(q_rl)
rot_ll = quatFromConfig(q_ll)
MRF.rotation = rot_rl.matrix()
MLF.rotation = rot_ll.matrix()
# apply the transform ankle -> center of contact
MRF *= fb.MRsole_offset
MLF *= fb.MLsole_offset
# hands
MRH = SE3()
MLH = SE3()
MRH.translation = np.matrix(q_rh[0:3]).T
MLH.translation = np.matrix(q_lh[0:3]).T
rot_rh = quatFromConfig(q_rh)
rot_lh = quatFromConfig(q_lh)
MRH.rotation = rot_rh.matrix()
MLH.rotation = rot_lh.matrix()
MRH *= fb.MRhand_offset
MLH *= fb.MLhand_offset
phase_d.RF_patch.placement = MRF
phase_d.LF_patch.placement = MLF
phase_d.RH_patch.placement = MRH
phase_d.LH_patch.placement = MLH
# initial state : Set all new contacts patch (either with placement computed below or unused)
if stateId == beginId:
# FIXME : for loop ? how ?
if fb.isLimbInContact(fb.rLegId, stateId):
phase_d.RF_patch.active = True
else:
phase_d.RF_patch.active = False
if fb.isLimbInContact(fb.lLegId, stateId):
phase_d.LF_patch.active = True
else:
phase_d.LF_patch.active = False
if fb.isLimbInContact(fb.rArmId, stateId):
phase_d.RH_patch.active = True
else:
phase_d.RH_patch.active = False
if fb.isLimbInContact(fb.lArmId, stateId):
phase_d.LH_patch.active = True
else:
phase_d.LH_patch.active = False
else:
# we need to copy the unchanged patch from the last simple support phase (and not create a new one with the same placement)
phase_d.RF_patch = phase_s.RF_patch
phase_d.RF_patch.active = fb.isLimbInContact(fb.rLegId, stateId)
phase_d.LF_patch = phase_s.LF_patch
phase_d.LF_patch.active = fb.isLimbInContact(fb.lLegId, stateId)
phase_d.RH_patch = phase_s.RH_patch
phase_d.RH_patch.active = fb.isLimbInContact(fb.rArmId, stateId)
phase_d.LH_patch = phase_s.LH_patch
phase_d.LH_patch.active = fb.isLimbInContact(fb.lArmId, stateId)
# now we change the contacts that have moved :
variations = fb.getContactsVariations(stateId - 1, stateId)
if len(variations) != 1:
print "Several contact changes between states " + str(
stateId -
1) + " and " + str(stateId) + " : " + str(variations)
assert len(
variations
) == 1, "Several changes of contacts in adjacent states, not implemented yet !"
for var in variations:
# FIXME : for loop in variation ? how ?
if var == fb.lLegId:
phase_d.LF_patch.placement = MLF
if var == fb.rLegId:
phase_d.RF_patch.placement = MRF
if var == fb.lArmId:
phase_d.LH_patch.placement = MLH
if var == fb.rArmId:
phase_d.RH_patch.placement = MRH
# retrieve the COM position for init and final state (equal for double support phases)
init_state = np.matrix(np.zeros(9)).T
init_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose()
init_state[3:6] = np.matrix(configs[config_id][-6:-3]).transpose()
final_state = init_state.copy()
#phase_d.time_trajectory.append((fb.getDurationForState(stateId))*cfg.DURATION_n_CONTACTS/cfg.SPEED)
phase_d.init_state = init_state
phase_d.final_state = final_state
phase_d.reference_configurations.append(
np.matrix((configs[config_id])).T)
#print "done for double support"
if stateId < endId:
# %%%%%% simple support : %%%%%%%%
phase_s = cs.contact_phases[cs_id + 1]
# copy previous placement :
phase_s.RF_patch = phase_d.RF_patch
phase_s.LF_patch = phase_d.LF_patch
phase_s.RH_patch = phase_d.RH_patch
phase_s.LH_patch = phase_d.LH_patch
# find the contact to break :
variations = fb.getContactsVariations(stateId, stateId + 1)
if len(variations) != 1:
print "Several contact changes between states " + str(
stateId) + " and " + str(stateId +
1) + " : " + str(variations)
assert len(
variations
) == 1, "Several changes of contacts in adjacent states, not implemented yet !"
for var in variations:
if var == fb.lLegId:
phase_s.LF_patch.active = False
if var == fb.rLegId:
phase_s.RF_patch.active = False
if var == fb.lArmId:
phase_s.LH_patch.active = False
if var == fb.rArmId:
phase_s.RH_patch.active = False
# retrieve the COM position for init and final state
phase_s.reference_configurations.append(
np.matrix((configs[config_id])).T)
init_state = phase_d.init_state.copy()
final_state = phase_d.final_state.copy()
fb.setCurrentConfig(configs[config_id + 1])
final_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose()
final_state[3:6] = np.matrix(configs[config_id +
1][-6:-3]).transpose()
#phase_s.time_trajectory.append((fb.getDurationForState(stateId))*(1-cfg.DURATION_n_CONTACTS)/cfg.SPEED)
phase_s.init_state = init_state.copy()
phase_s.final_state = final_state.copy()
#print "done for single support"
# assign contact models :
# only used by muscod ?? But we need to fill it otherwise the serialization fail
for k, phase in enumerate(cs.contact_phases):
RF_patch = phase.RF_patch
cm = RF_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
RF_patch.contactModelPlacement = SE3.Identity()
LF_patch = phase.LF_patch
cm = LF_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
LF_patch.contactModelPlacement = SE3.Identity()
LH_patch = phase.LH_patch
cm = LH_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
LH_patch.contactModelPlacement = SE3.Identity()
RH_patch = phase.RH_patch
cm = RH_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
RH_patch.contactModelPlacement = SE3.Identity()
return cs
'''
This function load a UR5 robot n a new model, move the basis to placement <M0>
and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
It returns the robot wrapper (model,data).
'''
robot = RobotWrapper(urdf, [PKG])
robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
robot.initDisplay(loadModel=True, viewerRootNodeName="world/" + name)
return robot
robots = []
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
Mt = SE3(eye(3), np.matrix([.3, 0, 0]).T) # First robot is simply translated
for i in range(4):
robots.append(loadRobot(SE3(rotate('z', np.pi / 2 * i), zero(3)) * Mt, "robot%d" % i))
# Set up the robots configuration with end effector pointed upward.
q0 = np.matrix([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0]).T
for i in range(4):
robots[i].display(q0)
# Add a new object featuring the parallel robot tool plate.
gepettoViewer = robots[0].viewer.gui
w, h, d = 0.25, 0.25, 0.005
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
gepettoViewer.addBox('world/toolplate', w, h, d, color)
Mtool = SE3(rotate('z', 1.268), np.matrix([0, 0, .77]).T)
gepettoViewer.applyConfiguration('world/toolplate', se3ToXYZQUATtuple(Mtool))
def generateContactSequence():
RF, root_init, pb, coms, footpos, allfeetpos, res = runLPScript()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, False)
q_init = fb.referenceConfig[::] + [0] * 6
q_init[0:7] = root_init
#q_init[2] += fb.referenceConfig[2] - 0.98 # 0.98 is in the _path script
v(q_init)
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequenceHumanoid(0)
addPhaseFromConfig(fb, v, cs, q_init, [fb.rLegId, fb.lLegId])
# loop over all phases of pb and add them to the cs :
for pId in range(
2, len(pb["phaseData"])
): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contact_phases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += 0.005 # FIXME it shouldn't be required !!
# compute desired foot rotation :
if USE_ORIENTATION:
if pId < len(pb["phaseData"]) - 1:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(
prev_contactPhase.LF_patch.placement.rotation)
else:
ql = rot
qr = Quaternion(
prev_contactPhase.RF_patch.placement.rotation)
rpy = matrixToRpy((qr * (ql.inverse())).matrix(
)) # rotation from the left foot pose to the right one
if rpy[2, 0] > 0: # yaw positive, feet are crossing
rot = Quaternion(phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = Quaternion(phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.matrix(pos).T
placement.rotation = rot.matrix()
moveEffectorToPlacement(fb,
v,
cs,
movingID,
placement,
initStateCenterSupportPolygon=True)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = fb.referenceConfig[::] + [0] * 6
p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
for i in range(3):
q_end[i] += p_end[i]
setFinalState(cs, q=q_end)
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v
def transQuatToSE3(p):
from pinocchio import SE3, Quaternion
from numpy import matrix
return SE3(
Quaternion(p[6], p[3], p[4], p[5]).matrix(),
matrix(p[0:3]).transpose())
def histogram(self):
# Build and display an histogram of the measurement errors stored
# in self.value
# the output is stored in self.errors
import matplotlib.pyplot as plt
self.errors = np.zeros(len(self.value)/6)
for i in range(len(self.errors)):
self.errors[i] = norm(self.value[6*i:6*i+6])
fig,axs = plt.subplots(1,1)
axs.hist(self.errors, bins=20)
fig.show()
if __name__ == '__main__':
filename = os.getenv('DEVEL_HPP_DIR') + \
'/install/share/talos_data/urdf/pyrene.urdf'
nj = len(ComputeCalibration.joints)
q_off = np.array(nj*[0.]).reshape(nj, 1)
hTc = SE3(Quaternion(x=-0.500, y=0.500, z=-0.500, w=0.500),
np.array([0.066, 0.013, 0.198]).reshape(3,1))
lwTls = SE3(Quaternion(x=0, y=0, z=0, w=1),
np.array([0.000, 0.000, -0.092]).reshape(3,1))
rwTrs = SE3(Quaternion(x=0, y=0, z=1, w=0),
np.array([0.000, 0.000, -0.092]).reshape(3,1))
cc=ComputeCalibration(filename, Variable(q_off,hTc,lwTls,rwTrs))
cc.readData('data/measurements-pyrene-20200819-1-filtered.csv')
for i in range (20):
cc.solve()
print ("||error|| = {}".format(norm(cc.value)))