def addPhaseFromConfig(fb, v, cs, q, limbsInContact):
phase = ContactPhaseHumanoid()
phase.reference_configurations.append(np.matrix((q)).T)
if v:
v(q)
fb.setCurrentConfig(q)
state = np.matrix(np.zeros(9)).T
state[0:3] = np.matrix(fb.getCenterOfMass()).T
phase.init_state = state.copy()
phase.final_state = state.copy()
# set Identity to each contact placement (otherwise it's uninitialized)
phase.RF_patch.placement = SE3.Identity()
phase.LF_patch.placement = SE3.Identity()
phase.RH_patch.placement = SE3.Identity()
phase.LH_patch.placement = SE3.Identity()
for limb in limbsInContact:
eeName = fb.dict_limb_joint[limb]
q_j = fb.getJointPosition(eeName)
patch = contactPatchForEffector(phase, eeName, fb)
placement = SE3FromConfig(q_j)
patch.placement = placement.act(fb.dict_offset[eeName])
patch.active = True
cs.contact_phases.append(phase)
if v:
display_tools.displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
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 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 walk(fb, cs, distance, stepLength, gait, duration_ss = -1 , duration_ds = -1, first_half_step=True):
"""
Generate a walking motion from the last phase in the contact sequence.
The contacts will be moved in the order of the 'gait' list. With the first one move only of half the stepLength
TODO : make it generic ! it's currently limited to motion in the x direction
:param fb: an rbprm.Fullbody object
:param cs: a ContactSequence
:param distance: the required distance the first and last contact placement along the X axis
:param stepLength: the length of the steps
:param gait: a list of limb names used as gait
:param duration_ss: the duration of the single support phases
:param duration_ds: the duration of the double support phases
"""
fb.usePosturalTaskContactCreation(True)
prev_phase = cs.contactPhases[-1]
for limb in gait:
eeName = fb.dict_limb_joint[limb]
assert prev_phase.isEffectorInContact(eeName), "All limbs in gait should be in contact in the first phase"
isFirst = first_half_step
reached = False
firstContactReachedGoal = False
remainingDistance = distance
while remainingDistance >= 1e-6:
for k, limb in enumerate(gait):
#print("move limb : ",limb)
eeName = fb.dict_limb_joint[limb]
if isFirst:
length = stepLength / 2.
isFirst = False
else:
length = stepLength
if k == 0 and first_half_step:
if length > (remainingDistance + stepLength / 2.):
length = remainingDistance + stepLength / 2.
firstContactReachedGoal = True
else:
if length > remainingDistance:
length = remainingDistance
transform = SE3.Identity()
#print("length = ",length)
transform.translation = np.array([length, 0, 0])
cs.moveEffectorOf(eeName, transform, duration_ds, duration_ss)
remainingDistance -= stepLength
if first_half_step and not firstContactReachedGoal:
transform = SE3.Identity()
#print("last length = ", stepLength)
transform.translation = np.array([stepLength / 2., 0, 0])
cs.moveEffectorOf(fb.dict_limb_joint[gait[0]], transform, duration_ds, duration_ss)
q_end = fb.referenceConfig[::] + [0] * 6
q_end[0] += distance
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
fb.usePosturalTaskContactCreation(False)
def __init__(self, robot, frame_id, ref_trajectory, name="SE3 Task"):
Task.__init__(self, robot, name)
self._frame_id = frame_id
self._ref_trajectory = ref_trajectory
# set default value to M_ref
self._M_ref = SE3.Identity()
# mask over the desired euclidian axis
self._mask = (np.ones(6)).astype(bool)
self._gMl = SE3.Identity()
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 __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 dyn_value(self, t, q, v):
g = self.robot.biais(q,0*v)
b = self.robot.biais(q,v)
b -= g;
M = self.robot.data.mass[0]#(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
b_com = cXi.inverse().np.T * b[:6,0]
b_angular = -b_com[3:,:]
M_com = cXi.inverse().np.T * M[:6,:]
L = M_com[3:,:] * v
L_des, Ldot_des = self._ref_traj(t)
L_error = L - L_des
acc = Ldot_des - b_com[3:,:]
self.a_des = acc
self.drift = 0*self.a_des
self._jacobian = self.jacobian(q)
return self._jacobian[self._mask,:], self.drift[self._mask], self.a_des[self._mask]
def displayEffectorTrajectories(cs,
viewer,
Robot,
suffixe="",
colorAlpha=1,
applyOffset=False):
"""
Display all the effector trajectories stored in the given ContactSequence.
With colors for each effectors defined in Robot.dict_limb_color_traj
:param cs: the ContactSequence storing the trajectories
:param viewer: An instance of hpp.gepetto.Viewer
:param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot)
:param suffixe: an optionnal suffixe to the name of the node objects created
:param colorAlpha: the transparency of the trajectories (must be between 0 and 1)
"""
effectors = cs.getAllEffectorsInContact()
for pid, phase in enumerate(cs.contactPhases):
for eeName in effectors:
if phase.effectorHaveAtrajectory(eeName):
color = Robot.dict_limb_color_traj[eeName]
color[-1] = colorAlpha
if applyOffset:
offset = -Robot.dict_offset[eeName]
else:
offset = SE3.Identity()
displaySE3Traj(phase.effectorTrajectory(eeName),
viewer.client.gui, viewer.sceneName,
eeName + "_traj_" + suffixe + str(pid), color,
[phase.timeInitial, phase.timeFinal], offset)
def dyn_value(self, t, q, v):
#(hg_ref, vhg_ref, ahg_ref) = self._ref_traj(t)
vhg_ref = np.matrix([0., 0., 0., 0., 0., 0.]).T
model = self.robot.model
data = self.robot.data
JMom = se3.ccrba(model, data, q, v)
hg_prv = data.hg.vector.copy()[self._mask,:]
#self.p_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:]
#self.v_error = self.robot.fext[self._mask,:] - vhg_ref[self._mask,:]
#self.v_error = self.robot.fext[self._mask,:]
#self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:]
#self.a_des = -self.kv*self.v_error #+model.gravity.vector[self._mask,:]
self.a_des = self.kv*self.robot.fext[self._mask,:]#vhg_ref #+model.gravity.vector[self._mask,:]
#self.drift = 0 * self.a_des
#self.a_des =
#***********************
p_com = data.com[0]
cXi = SE3.Identity()
oXi = self.robot.data.oMi[1]
cXi.rotation = oXi.rotation
cXi.translation = oXi.translation - p_com
m_gravity = model.gravity.copy()
model.gravity.setZero()
b = se3.nle(model,data,q,v)
model.gravity = m_gravity
f_ff = se3.Force(b[:6])
f_com = cXi.act(f_ff)
hg_drift = f_com.angular
self.drift=f_com.np[self._mask,:]
#************************
#print JMom.copy()[self._mask,:].shape
#print self.__gain_matrix.shape
self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix
return self._jacobian, self.drift, self.a_des
def create_stairs_cs():
ENV_NAME = "multicontact/bauzil_stairs"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
cs = ContactSequence(0)
# Create an initial contact phase :
q_ref = fb.referenceConfig[::] + [0] * 6
q_ref[0:2] = [0.07, 1.2]
addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId])
step_height = 0.1
step_width = 0.3
displacement = SE3.Identity()
displacement.translation = array([step_width, 0, step_height])
cs.moveEffectorOf(fb.rfoot, displacement)
cs.moveEffectorOf(fb.lfoot, displacement)
q_end = q_ref[::]
q_end[0] += step_width
q_end[2] += step_height
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, array(com), q=q_end)
return cs
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 dyn_value(self, t, q, v, update_geometry=False):
g = self.robot.bias(q, 0 * v)
b = self.robot.bias(q, v)
b -= g
M = 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
b_com = cXi.actInv(se3.Force(b[:6, 0]))
# b_com = cXi.actInv(b[:6,0]).vector
b_com = b_com.angular
# M_com = cXi.inverse().action.T * M[:6,:]
# M_com = cXi.inverse().np.T * M[:6,:]
M_com = self.robot.momentumJacobian(q, v, update_geometry)
M_com = M_com[3:, :]
L = M_com * v
L_ref, dL_ref, ddL_ref = self._ref_traj(t)
# acc = dL_ref - b_com[3:,:]
dL_des = dL_ref - self.kp * (L - L_ref)
return M_com[self._mask, :], b_com[self._mask, :], dL_des[self._mask,
0]
def displaySE3Traj(traj,
gui,
sceneName,
name,
color,
time_interval,
offset=SE3.Identity()):
if name == None:
name = "SE3_traj"
rootName = name
# add indices until the name is free
list = gui.getNodeList()
i = 0
while list.count(name) > 0:
name = rootName + "_" + str(i)
i += 1
path = []
dt = 0.01
t = time_interval[0]
while t <= time_interval[1]:
m = traj(t)[0]
m = m.act(offset)
path += m.translation.T.tolist()
t += dt
gui.addCurve(name, path, color)
gui.addToGroup(name, sceneName)
gui.refresh()
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, 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 walk(fb, cs, distance, stepLength, gait, duration_ss=-1, duration_ds=-1):
fb.usePosturalTaskContactCreation(True)
prev_phase = cs.contactPhases[-1]
for limb in gait:
eeName = fb.dict_limb_joint[limb]
assert prev_phase.isEffectorInContact(
eeName
), "All limbs in gait should be in contact in the first phase"
isFirst = True
reached = False
firstContactReachedGoal = False
remainingDistance = distance
while remainingDistance >= 1e-6:
for k, limb in enumerate(gait):
#print("move limb : ",limb)
eeName = fb.dict_limb_joint[limb]
if isFirst:
length = stepLength / 2.
isFirst = False
else:
length = stepLength
if k == 0:
if length > (remainingDistance + stepLength / 2.):
length = remainingDistance + stepLength / 2.
firstContactReachedGoal = True
else:
if length > remainingDistance:
length = remainingDistance
transform = SE3.Identity()
#print("length = ",length)
transform.translation = np.array([length, 0, 0])
cs.moveEffectorOf(eeName, transform, duration_ds, duration_ss)
remainingDistance -= stepLength
if not firstContactReachedGoal:
transform = SE3.Identity()
#print("last length = ", stepLength)
transform.translation = np.array([stepLength / 2., 0, 0])
cs.moveEffectorOf(fb.dict_limb_joint[gait[0]], transform, duration_ds,
duration_ss)
q_end = fb.referenceConfig[::] + [0] * 6
q_end[0] += distance
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
fb.usePosturalTaskContactCreation(False)
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 computeWrenchRef(res):
Mcom = SE3.Identity()
for k, t in enumerate(res.t_t):
Mcom.translation = res.c_reference[:, k]
Fcom = Force.Zero()
Fcom.linear = cfg.MASS * (res.ddc_reference[:, k] - cfg.GRAVITY)
Fcom.angular = res.dL_reference[:, k]
F0 = Mcom.act(Fcom)
res.wrench_reference[:, k] = F0.vector
return res
def computeInequalitiesAroundLine(fullBody, p_from, p_to, eeName, groupName,
viewer):
a = p_from.translation.tolist()
b = p_to.translation.tolist()
# size of the end effector (-x,x,-y,y,-z,z)
size_diagonal = math.sqrt(
cfg.Robot.dict_size[eeName][0]**2 +
cfg.Robot.dict_size[eeName][1]**2) #TODO margin ??
#size = [size_diagonal/2., size_diagonal/2.,0.001]
size = [
-cfg.Robot.dict_size[eeName][0] / 2.,
cfg.Robot.dict_size[eeName][0] / 2.,
-cfg.Robot.dict_size[eeName][1] / 2.,
cfg.Robot.dict_size[eeName][1] / 2., -0.001, 0.001
]
#size=[0,0,0] #FIXME debug
"""
size_r = np.array(size)
# rotate size vector according to initial rotation of the effector :
if VERBOSE :
print "rotation init : ",p_from.rotation
size_r = p_from.rotation.dot(size_r)
"""
points = large_col_free_box(fullBody.clientRbprm.rbprm,
a,
b,
sizeObject=size)
# Display the box before the size reduction :
#if DISPLAY_CONSTRAINTS and not DISPLAY_JOINT_LEVEL:
# display_box(viewer,points,groupName)
"""
pointsReduced = []
rot_init = p_from.rotation
for i in range(len(box_points)):
#pointsReduced += [points[i]-rot_init.dot((size_r*array(box_points[i]))/2.)]
if VERBOSE :
print "for point "+str(i)+" shift of "+str(-((size_r*np.array(box_points[i]))))
pointsReduced += [points[i]-((size_r*np.array(box_points[i])))]
"""
# display the box after size reduction
if DISPLAY_CONSTRAINTS and not DISPLAY_JOINT_LEVEL:
display_box(viewer, points, groupName)
# transform the points back to joint level
pc = SE3.Identity(
) # take Identity rotation #FIXME probably not the best idea ...
pointsJoint = []
for point in points:
pc.translation = np.matrix(point).T
pointsJoint += [
cfg.Robot.dict_offset[eeName].actInv(pc).translation.tolist()
]
if DISPLAY_CONSTRAINTS and DISPLAY_JOINT_LEVEL:
display_box(viewer, pointsJoint, groupName)
H, h = to_ineq(pointsJoint)
return H, h.reshape(-1, 1)
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 SE3FromVec(vect):
if vect.shape[0] != 12 or vect.shape[1] != 1:
raise ValueError("SE3FromVect take as input a vector of size 12")
placement = SE3.Identity()
placement.translation = vect[0:3]
rot = placement.rotation
rot[:, 0] = np.asarray(vect[3:6]).reshape(-1)
rot[:, 1] = np.asarray(vect[6:9]).reshape(-1)
rot[:, 2] = np.asarray(vect[9:12]).reshape(-1)
placement.rotation = rot
return placement
def __init__(self, robot, frame_id, ref_trajectory, name = "Task"):
Task.__init__ (self, robot, name)
self._frame_id = frame_id
self._ref_trajectory = ref_trajectory
# set default value to M_ref
self._M_ref = SE3.Identity
# mask over the desired euclidian axis
self._mask = (np.ones(6)).astype(bool)
# for local to global
self._gMl = SE3.Identity()
self.__gain_matrix = np.matrix(np.eye(robot.nv))
def __init__(self, robot, frame_id, op_point, target, ref_trajectory, name = "Task"):
Task.__init__ (self, robot, name)
self._frame_id = frame_id
self._ref_trajectory = ref_trajectory
self._op_point = op_point
# set default value to M_ref
self._M_ref = SE3.Identity
# mask over the desired euclidian axis
self._mask = (np.ones(6)).astype(bool)
self._target = target
# for local to global
self._gMl = SE3.Identity()
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.length:
print "Trajectory called after final time."
return self.compute_for_normalized_time(self.t_total)
u = t / self.length
self.M = SE3.Identity()
self.M.translation = u * self.placement_final.translation + (
1. - u) * self.placement_init.translation
self.M.rotation = (self.quat0.slerp(u, self.quat1)).matrix()
return self.M, self.v, self.a
class Robot: # data for talos (to avoid a depencie on talos-rbprm for this script)
rfoot = 'leg_right_6_joint'
lfoot = 'leg_left_6_joint'
rhand = 'arm_right_7_joint'
lhand = 'arm_left_7_joint'
rLegOffset = [0., -0.00018, -0.102]
lLegOffset = [0., -0.00018, -0.102]
rArmOffset = [-0.01, 0., -0.154]
lArmOffset = [-0.01, 0., -0.154]
MRsole_offset = SE3.Identity()
MRsole_offset.translation = np.matrix(rLegOffset).T
MLsole_offset = SE3.Identity()
MLsole_offset.translation = np.matrix(lLegOffset).T
MRhand_offset = SE3.Identity()
MRhand_offset.translation = np.matrix(rArmOffset).T
MLhand_offset = SE3.Identity()
MLhand_offset.translation = np.matrix(lArmOffset).T
dict_offset = {
rfoot: MRsole_offset,
lfoot: MLsole_offset,
rhand: MRhand_offset,
lhand: MLhand_offset
}
MRsole_display = SE3.Identity()
MLsole_display = SE3.Identity()
MRhand_display = SE3.Identity()
#MRhand_display.translation = np.matrix([0, 0., -0.11])
MLhand_display = SE3.Identity()
#MLhand_display.translation = np.matrix([0, 0., -0.11])
dict_display_offset = {
rfoot: MRsole_display,
lfoot: MLsole_display,
rhand: MRhand_display,
lhand: MLhand_display
}
dict_limb_color_traj = {
rfoot: [0, 1, 0, 1],
lfoot: [1, 0, 0, 1],
rhand: [0, 0, 1, 1],
lhand: [0.9, 0.5, 0, 1]
}
dict_size = {
rfoot: [0.2, 0.13],
lfoot: [0.2, 0.13],
rhand: [0.1, 0.1],
lhand: [0.1, 0.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 computeBoxVertices(client, center, x_dir, dim, sizeObject):
# compute the vertices of this box:
#points = [ [dim[1],-dim[2],-dim[4]], [dim[1],-dim[2],dim[5]], [-dim[0],-dim[2],dim[5]], [-dim[0],-dim[2],-dim[4]], [dim[1],dim[3],-dim[4]], [dim[1],dim[3],dim[5]], [-dim[0],dim[3],dim[5]], [-dim[0],dim[3],-dim[4]] ]
points = []
for sign in box_points:
point = []
for i in range(3):
if sign[i] < 0: # required because dimensions are not symetrical
point += [-dim[i * 2]]
else:
point += [dim[i * 2 + 1]]
points += [point]
# transform this points to the position/orientation of the box :
rot = rot_mat_x(client, x_dir.tolist())
t_c_w = SE3.Identity() # transform center of box in world frame
t_c_w.translation = np.matrix(center).T
t_c_w.rotation = rot
pointsTransform = []
for p in points:
t_p_c = SE3.Identity() # vertice position in box frame
t_p_c.translation = np.matrix(p).T
pointsTransform += [t_c_w.act(t_p_c).translation.T[0]]
return pointsTransform
def createRandomState(fullBody, limbsInContact):
extraDof = int(fullBody.client.robot.getDimensionExtraConfigSpace())
q0 = fullBody.referenceConfig[::]
if extraDof > 0:
q0 += [0] * extraDof
qr = fullBody.shootRandomConfig()
root = SE3.Identity()
root.translation = np.matrix(qr[0:3]).T
# sample random orientation along z :
root = sampleRotationAlongZ(root)
q0[0:7] = se3ToXYZQUATtuple(root)
# apply random config to legs (FIXME : ID hardcoded for Talos)
q0[7:19] = qr[7:19]
fullBody.setCurrentConfig(q0)
s0 = State(fullBody, q=q0, limbsIncontact=limbsInContact)
return s0
