def rootOrientationFromFeetPlacement(phase, phase_next):
#FIXME : extract only the yaw rotation
qr = Quaternion(phase.RF_patch.placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
ql = Quaternion(phase.LF_patch.placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
q_rot = qr.slerp(0.5, ql)
placement_init = SE3.Identity()
placement_init.rotation = q_rot.matrix()
if phase_next:
if not isContactActive(phase, cfg.Robot.rfoot) and isContactActive(
phase_next, cfg.Robot.rfoot):
qr = Quaternion(phase_next.RF_patch.placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
if not isContactActive(phase, cfg.Robot.lfoot) and isContactActive(
phase_next, cfg.Robot.lfoot):
ql = Quaternion(phase_next.LF_patch.placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
q_rot = qr.slerp(0.5, ql)
placement_end = SE3.Identity()
placement_end.rotation = q_rot.matrix()
return placement_init, placement_end
def compute_orientation_for_feet_placement(fb, pb, pId, moving, RF, prev_contactPhase, use_interpolated_orientation):
"""
Compute the rotation of the feet from the base orientation.
:param fb: an instance of rbprm.Fullbody
:param pb: the SL1M problem dictionary, containing all the phaseData
:param pId: the Id of the current phase (SL1M index)
:param moving: the Id of the moving feet
:param RF: the Id of the right feet in the SL1M solver
:param prev_contactPhase: the multicontact_api.ContactPhase of the previous phase
:param use_interpolated_orientation: If False, the desired contact rotation is the current base orientation.
If True, the desired contact rotation is the interpolation between the current base orientation
and the one for the next phase
:return: the rotation matrix of the new contact placement
"""
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 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: # yaw positive, feet are crossing
rot = quat0 # rotation of the root, from the guide
else:
rot = quat0 # rotation of the root, from the guide
return rot
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 computeRootYawAngleBetwwenConfigs(q0, q1):
quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5])
quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5])
v_angular = np.array(log3(quat0.matrix().T.dot(quat1.matrix())))
#print ("q_prev : ",q0)
#print ("q : ",q1)
#print ("v_angular = ",v_angular)
return v_angular[2]
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 gen_state(s, pId, num_max_sample = 0, first = False, normal = lp.Z_AXIS, newContact = True ,projectCOM = True):
#~ pId = 6
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fullBody.lLegId
if moving == lp.RF:
movingID = fullBody.rLegId
print("# gen state for phase Id = ",pId)
if False and pId < len(pb["phaseData"])-1:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
quat1 = Quaternion(pb["phaseData"][pId+1]["rootOrientation"])
qrot = (quat0.slerp(0.5,quat1)).matrix()
else:
qrot = Quaternion(phase["rootOrientation"]) # rotation of the root, from the guide
#q_n = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T)
#rot = quatToConfig(qrot * q_n)
if not isclose(normal,Z_AXIS).all():
qrot = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T)
# ignore guide orientation when normal is not z ...
#rot = quatToConfig(qrot)
pos = allfeetpos[pId];
pos[2] += 0.002
pose = pos.tolist()+quatToConfig(qrot)
print("Try to add contact for "+movingID+" pos = "+str(pose))
disp.moveSphere('c',v,pose)
if newContact:
sres, succ = tryCreateContactAround(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample,rotation = qrot)
#sres, succ = StateHelper.removeContact(s,movingID)
#assert succ
#succ = sres.projectToRoot(s.q()[0:3]+rot)
#sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample)
else:
sres, succ = StateHelper.cloneState(s)
if not succ:
print("Cannot project config q = ",sres.q())
print("To new contact position for "+movingID+" = "+str(pose)+" ; n = "+str(normal.tolist()))
raise RuntimeError("Cannot project feet to new contact position") # try something else ??
if projectCOM :
#sfeet, _ = StateHelper.cloneState(sres)
#print "config before projecting to com q1=",sres.q()
successCOM = projectCoMInSupportPolygon(sres)
if not successCOM:
# is it really an issue ?
print("Unable to project CoM in the center of the support polygone")
v(sres.q())
return sres
def SE3ToViewerConfig(placement):
q = [0] * 7
q[0:3] = placement.translation.T.tolist()[0]
r = Quaternion(placement.rotation)
q[6] = r.w
q[3:6] = r.coeffs().transpose().tolist()[0][0:3]
return q
def callback_robotState(data):
global cpt_1
cpt_1 += 1
if (cpt_1 % DOWNSAMPLING):
return
#read vector q from ros
q = np.array(data.data)
#construct qUrdf from sot convention and eventualy external freeflyer
qUrdf = np.zeros(37)
if READ_FREEFLYER_FROM_AA_SIGNAL:
global ff_xzyquat
qUrdf[:7] = ff_xzyquat
else:
qUrdf[:3] = q[:3]
quatMat = se3.utils.rpyToMatrix(np.matrix(q[3:6]).T)
quatVec = Quaternion(quatMat)
qUrdf[3:7] = np.array(quatVec.coeffs()).squeeze()
qUrdf[7:11] = q[18:22]
# chest-head
qUrdf[11:18] = q[29:]
# larm
qUrdf[18:25] = q[22:29]
# rarm
qUrdf[25:31] = q[12:18]
# lleg
qUrdf[31:] = q[6:12]
# rleg
global q_glob
q_glob = qUrdf #update global configuration vector
#~ embed()
#~ tau = se3.rnea(v.robot.model,v.robot.data,qUrdf,np.matlib.zeros(37),np.matlib.zeros(37))
#~ print tau[-4]
v.updateRobotConfig(q_glob)
def compute_freeflyer_state_from_fixed_body(jiminy_model,
fixed_body_name,
position,
velocity=None,
acceleration=None,
use_theoretical_model=True):
"""
@brief Fill rootjoint data from articular data when a body is fixed parallel to world.
@details The hypothesis is that 'fixed_body_name' is fixed parallel to world frame.
So this method computes the position of freeflyer rootjoint in the fixed body frame.
@note This function modifies internal data.
@param model The Jiminy model
@param fixed_body_name The name of the body that is considered fixed parallel to world frame
@param[inout] position Must contain current articular data. The rootjoint data can
contain any value, it will be ignored and replaced
The method fills in rootjoint data
@param[inout] velocity Same as positionInOut but for velocity
@pre Rootjoint must be a freeflyer
@pre positionInOut.size() == model_->nq
"""
if use_theoretical_model:
pnc_model = jiminy_model.pinocchio_model_th
pnc_data = jiminy_model.pinocchio_data_th
else:
pnc_model = jiminy_model.pinocchio_model
pnc_data = jiminy_model.pinocchio_data
position[:6].fill(0)
position[6] = 1.0
if velocity is not None:
velocity[:6].fill(0)
else:
velocity = np.zeros((pnc_model.nv, ))
if acceleration is not None:
acceleration[:6].fill(0)
else:
acceleration = np.zeros((pnc_model.nv, ))
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity,
acceleration)
pnc.framesForwardKinematics(pnc_model, pnc_data, position)
ff_M_fixed_body = get_body_world_transform(jiminy_model, fixed_body_name)
w_M_ff = ff_M_fixed_body.inverse()
base_link_translation = w_M_ff.translation
base_link_quaternion = Quaternion(w_M_ff.rotation)
position[:3] = base_link_translation
position[3:7] = base_link_quaternion.coeffs()
ff_v_fixed_body = get_body_world_velocity(jiminy_model, fixed_body_name)
base_link_velocity = -ff_v_fixed_body
velocity[:6] = base_link_velocity.vector
ff_a_fixedBody = get_body_world_acceleration(jiminy_model, fixed_body_name)
base_link_acceleration = -ff_a_fixedBody
acceleration[:6] = base_link_acceleration.vector
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 refresh_observation(self) -> None:
if not self.simulator.is_simulation_running:
# Initialize observation chunks
self.obs_chunks = [
self.system_state.q[2:],
self.system_state.v,
*[f.vector for f in self.system_state.f_external]
]
# Initialize observation chunks sizes
self.obs_chunks_sizes = []
idx_start = 0
for obs in self.obs_chunks:
idx_end = idx_start + len(obs)
self.obs_chunks_sizes.append([idx_start, idx_end])
idx_start = idx_end
# Initialize previous torso position
self.xpos_prev = self.system_state.q[0]
# Update observation buffer
for obs, size in zip(self.obs_chunks, self.obs_chunks_sizes):
obs_idx = slice(*size)
low = self.observation_space.low[obs_idx]
high = self.observation_space.high[obs_idx]
self._observation[obs_idx] = np.clip(obs, low, high)
# Transform observed linear velocity to be in world frame
self._observation[slice(*self.obs_chunks_sizes[1])][:3] = \
Quaternion(self.system_state.q[3:7]) * self.obs_chunks[1][:3]
def compute_for_normalized_time(self, t):
if t < 0:
print "Trajectory called with negative time."
return self.compute_for_normalized_time(0)
elif t > self.t_total:
print "Trajectory called after final time."
return self.compute_for_normalized_time(self.t_total)
self.M = SE3.Identity()
self.v = Motion.Zero()
self.a = Motion.Zero()
self.M.translation = self.curves(t)
self.v.linear = self.curves.d(t)
self.a.linear = self.curves.dd(t)
#rotation :
if self.curves.isInFirstNonZero(t):
self.M.rotation = self.placement_init.rotation.copy()
elif self.curves.isInLastNonZero(t):
self.M.rotation = self.placement_end.rotation.copy()
else:
# make a slerp between self.effector_placement[id][0] and [1] :
quat0 = Quaternion(self.placement_init.rotation)
quat1 = Quaternion(self.placement_end.rotation)
t_rot = t - self.t_mid_begin
"""
print "t : ",t
print "t_mid_begin : ",self.t_mid_begin
print "t_rot : ",t_rot
print "t mid : ",self.t_mid
"""
assert t_rot >= 0, "Error in the time intervals of the polybezier"
assert t_rot <= (self.t_mid + 1e-6
), "Error in the time intervals of the polybezier"
u = t_rot / self.t_mid
# normalized time without the pre defined takeoff/landing phases
self.M.rotation = (quat0.slerp(u, quat1)).matrix()
# angular velocity :
dt = 0.001
u_dt = dt / self.t_mid
r_plus_dt = (quat0.slerp(u + u_dt, quat1)).matrix()
self.v.angular = pin.log3(self.M.rotation.T * r_plus_dt) / dt
r_plus2_dt = (quat0.slerp(u + (2. * u_dt), quat1)).matrix()
next_angular_velocity = pin.log3(r_plus_dt.T * r_plus2_dt) / dt
self.a.angular = (next_angular_velocity - self.v.angular) / dt
#r_plus_dt = (quat0.slerp(u+u_dt,quat1)).matrix()
#next_angular_vel = (pin.log3(self.M.rotation.T * r_plus_dt)/dt)
#self.a.angular = (next_angular_vel - self.v.angular)/dt
return self.M, self.v, self.a
def config_sot_to_urdf(q):
qUrdf = mat_zeros(39)
qUrdf[:3, 0] = q[:3, 0]
quatMat = rpyToMatrix(q[3:6, 0])
quatVec = Quaternion(quatMat)
qUrdf[3:7, 0] = quatVec.coeffs()
qUrdf[7:, 0] = q[6:, 0]
return qUrdf
def SE3FromConfig(q):
if isinstance(q, types.ListType):
q = np.matrix(q).T
placement = SE3.Identity()
placement.translation = q[0:3]
r = Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0])
placement.rotation = r.matrix()
return placement
def quatConfigFromMatrix(m):
"""
Transform a rotation matrix to a list containing the quaternion representation (x, y, z, w)
:param m: a rotation matrix
:return: The Quaternion stored as a list
"""
quat = Quaternion(m)
return quatToConfig(quat)
def __init__(self, placement_init, placement_final, time_interval):
RefTrajectory.__init__(self, "TrajectorySE3LinearInterp")
self.placement_init = placement_init
self.placement_final = placement_final
self.t0 = time_interval[0]
self.t1 = time_interval[1]
self.length = self.t1 - self.t0
self.quat0 = Quaternion(self.placement_init.rotation)
self.quat1 = Quaternion(self.placement_final.rotation)
self.M = SE3.Identity()
self.v = Motion.Zero()
self.a = Motion.Zero()
# constant velocity and null acceleration :
self.v.linear = (placement_final.translation -
placement_final.translation) / self.length
self.v.angular = pin.log3(placement_final.rotation.T *
placement_final.rotation) / self.length
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 SE3FromConfig(q):
if isinstance(q, list):
q = np.array(q)
placement = SE3.Identity()
tr = np.array(q[0:3])
placement.translation = tr
r = Quaternion(q[6], q[3], q[4], q[5])
placement.rotation = r.matrix()
return placement
def SE3ToViewerConfig(placement):
"""
Convert a pinocchio.SE3 object to a python list of lenght 7 : translation + quaternion (x, y, z, w)
:param placement: the pinocchio.SE3 object
:return: a list of lenght 7
"""
q = [0] * 7
q[0:3] = placement.translation.tolist()
r = Quaternion(placement.rotation)
q[6] = r.w
q[3:6] = r.coeffs().tolist()[0:3]
return q
def config_sot_to_urdf(q):
# GEPETTO VIEWER Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36
# ROBOT VIEWER # Free flyer0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35
qUrdf = mat_zeros(37);
qUrdf[:3,0] = q[:3,0];
quatMat = rpyToMatrix(q[3:6,0]);
quatVec = Quaternion(quatMat);
qUrdf[3:7,0] = quatVec.coeffs();
qUrdf[7:11,0] = q[18:22,0]; # chest-head
qUrdf[11:18,0] = q[29:,0]; # larm
qUrdf[18:25,0] = q[22:29,0]; # rarm
qUrdf[25:31,0] = q[12:18,0]; # lleg
qUrdf[31:,0] = q[6:12,0]; # rleg
return qUrdf;
def plan_guide(self, root_goal):
"""
Plan a guide from the current last_phase root position to the given root position
:param root_goal: list of size 3 or 7: translation and quaternion for the desired root position
If the quaternion part is not specified, the final orientation is not constrained
Store the Id of the new path in self.current_guide_id
"""
self.guide_planner.q_goal = self.guide_planner.q_init[::]
self.guide_planner.q_goal[:3] = root_goal[:3]
self.guide_planner.q_goal[3:7] = [0, 0, 0, 1]
self.guide_planner.q_goal[-6:] = [0] * 6
last_phase = self.get_last_phase()
if last_phase:
logger.warning("Last phase not None")
logger.warning("Last phase q_final : %s", last_phase.q_final[:7])
self.guide_planner.q_init[:7] = last_phase.q_final[:7]
self.guide_planner.q_init[2] = self.guide_planner.rbprmBuilder.ref_height # FIXME
#add small velocity in order to have smooth change of orientation at the beginning/end
quat_init = Quaternion(self.guide_planner.q_init[6], self.guide_planner.q_init[3],
self.guide_planner.q_init[4], self.guide_planner.q_init[5])
dir_init = quat_init * np.array([1, 0, 0])
self.guide_planner.q_init[-6] = dir_init[0] * V_INIT
self.guide_planner.q_init[-5] = dir_init[1] * V_INIT
if len(root_goal) >= 7:
self.guide_planner.q_goal[3:7] = root_goal[3:7]
quat_goal = Quaternion(self.guide_planner.q_goal[6], self.guide_planner.q_goal[3],
self.guide_planner.q_goal[4], self.guide_planner.q_goal[5])
dir_goal = quat_goal * np.array([1, 0, 0])
self.guide_planner.q_goal[-6] = dir_goal[0] * V_GOAL
self.guide_planner.q_goal[-5] = dir_goal[1] * V_GOAL
logger.warning("Guide init = %s", self.guide_planner.q_init)
logger.warning("Guide goal = %s", self.guide_planner.q_goal)
self.guide_planner.ps.resetGoalConfigs()
self.guide_planner.ps.clearRoadmap()
self.current_root_goal = root_goal
self.guide_planner.solve(True)
self.current_guide_id = self.guide_planner.ps.numberPaths() - 1
def exportWaist(path, waist_t):
filename = path + "/WaistOrientation.dat"
with open(filename, 'w') as f:
for waist in waist_t.T:
quat = Quaternion(waist[0, 6], waist[0, 3], waist[0, 4], waist[0, 5])
#rot = matrixToRpy(quat.matrix()) # DEBUG
rot = matrixToRpy(SE3.Identity().rotation) # DEBUG
line = ""
for i in range(3):
line += str(rot[i, 0]) + " "
for i in range(6):
line += "0 "
f.write(line.rstrip(" ") + "\n")
print("Motion exported to : ", filename)
return
def SE3FromConfig(q):
"""
Convert a vector of size >=7 to a pinocchio.SE3 object.
Assume that the first 3 values of the vector are the translation part, followed by a quaternion(x,y,z,w)
:param q: a list or a numpy array of size >=7
:return: a SE3 object
"""
if isinstance(q, list):
q = np.array(q)
placement = SE3.Identity()
tr = np.array(q[0:3])
placement.translation = tr
r = Quaternion(q[6], q[3], q[4], q[5])
placement.rotation = r.matrix()
return placement
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
effectorsInContact = phase.effectorsInContact()
contacts = [
] # contacts should contains the limb names, not the effector names
list_effector = list(fb.dict_limb_joint.values())
for eeName in effectorsInContact:
contacts += [
list(fb.dict_limb_joint.keys())[list_effector.index(eeName)]
]
#q = phase.q_init.tolist() # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(
phase, fb.DEFAULT_COM_HEIGHT).tolist()
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(phase.root_t.evaluateAsSE3(phase.timeInitial).rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = phase.contactPatch(eeName).placement
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = phase.contactPatch(eeName).placement
p = placement.translation.tolist()
n = computeContactNormal(placement).tolist()
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print(
"Cannot project the configuration to contact, for effector : ",
eeName)
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print(
"cannot project com to the middle of the support polygon."
)
phase.q_init = np.array(state.q())
return state.q()
def computePoseFromSurface(surface):
points = surface
#normal = surface[1]
normal = [0,0,1]
center = np.zeros(3)
for pt in points:
center += np.array(pt)
center /= len(points)
center -= np.array(normal)*(WIDTH/2.)
# rotation in rpy :
q_rot = Quaternion()
n_m = np.matrix(normal).T
q_rot.setFromTwoVectors(Z_AXIS,n_m)
rpy = matrixToRpy(q_rot.matrix())
pose = center.tolist() + rpy.T.tolist()[0]
return pose
def pinocchio_2_sot(q):
# PINOCCHIO Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36
# SOT Free flyer 0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35
qSot = np.matlib.zeros((36, 1))
qSot[:3] = q[:3]
quatMat = Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix()
qSot[3:6] = matrixToRpy(quatMat)
qSot[18:22] = q[7:11]
# chest-head
qSot[29:] = q[11:18]
# larm
qSot[22:29] = q[18:25]
# rarm
qSot[12:18] = q[25:31]
# lleg
qSot[6:12] = q[31:]
# rleg
return qSot.A.squeeze()
def config_sot_to_urdf(q_sot):
q_sot = np.array(q_sot)
qUrdf = np.zeros(37)
qUrdf[:3] = q_sot[:3]
quatMat = se3.utils.rpyToMatrix(np.matrix(q_sot[3:6]).T)
quatVec = Quaternion(quatMat)
qUrdf[3:7] = np.array(quatVec.coeffs()).squeeze()
qUrdf[7:11] = q_sot[18:22]
# chest-head
qUrdf[11:18] = q_sot[29:36]
# larm
qUrdf[18:25] = q_sot[22:29]
# rarm
qUrdf[25:31] = q_sot[12:18]
# lleg
qUrdf[31:37] = q_sot[6:12]
# rleg
return qUrdf
def compute_stopping_cs(self, move_to_support_polygon=True):
"""
Compute a Contact Sequence with centroidal trajectories to bring the current last_phase
to a stop without contact changes
:param move_to_support_polygon: if True, add a trajectory to put the CoM above the center of the support polygon
:return:
"""
phase_stop = ContactPhase(self.get_last_phase())
tools.setInitialFromFinalValues(phase_stop, phase_stop)
phase_stop.timeInitial = phase_stop.timeFinal
phase_stop.duration = DURATION_0_STEP # FIXME !!
# try 0-step:
success, phase = zeroStepCapturability(phase_stop, self.cfg)
if success:
cs_ref = ContactSequence(0)
cs_ref.append(phase)
# TEST : add another phase to go back in the center of the support polygon
if move_to_support_polygon:
phase_projected = ContactPhase()
phase_projected.timeInitial = phase.timeFinal
phase_projected.duration = DURATION_0_STEP
tools.copyContactPlacement(phase, phase_projected)
tools.setInitialFromFinalValues(phase, phase_projected)
phase_projected.c_final = tools.computeCenterOfSupportPolygonFromPhase(
phase_stop, self.fullBody.DEFAULT_COM_HEIGHT)
#FIXME 'default height'
tools.connectPhaseTrajToFinalState(phase_projected)
cs_ref.append(phase_projected)
else:
# TODO try 1 step :
raise RuntimeError("One step capturability not implemented yet !")
tools.computeRootTrajFromContacts(self.fullBody, cs_ref)
self.last_phase = cs_ref.contactPhases[-1].copy()
# define the final root position, translation from the CoM position and rotation from the feet rotation
q_final = np.zeros(7)
q_final[:3] = self.last_phase.c_final[::]
placement_rot_root, _ = tools.rootOrientationFromFeetPlacement(self.cfg.Robot, None, self.last_phase, None)
quat_root = Quaternion(placement_rot_root.rotation)
q_final[3:7] = [quat_root.x, quat_root.y, quat_root.z, quat_root.w]
self.last_phase.q_final = q_final
self.last_phase_flag.value = False
self.last_phase_pickled = Array(c_ubyte, MAX_PICKLE_SIZE) # reset currently stored whole body last phase
return cs_ref
def sot_2_pinocchio(q):
# PINOCCHIO Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36
# SOT Free flyer 0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35
qPino = np.matlib.zeros((37, 1))
qPino[:3, 0] = q[:3]
quatMat = rpyToMatrix(q[3:6])
quatVec = Quaternion(quatMat)
qPino[3:7, 0] = quatVec.coeffs()
qPino[7:11, 0] = q[18:22]
# chest-head
qPino[11:18, 0] = q[29:]
# larm
qPino[18:25, 0] = q[22:29]
# rarm
qPino[25:31, 0] = q[12:18]
# lleg
qPino[31:, 0] = q[6:12]
# rleg
return qPino
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)
