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 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 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 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 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 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 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 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 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 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 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
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,:]
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
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 error_dyn(self, t, q, v):
g = self.robot.biais(q, 0 * v)
b = self.robot.biais(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.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:, :]
# Compute error
#error_value = self.__error_value
#error_value[:6,0] = error_ff
#error_value[6:,0] = q[7:,0] - self.q_posture_des[7:,0]
#print error_value
#diag = np.matrix(self.robot.data.M.diagonal())
#print diag
#M = self.robot.data.M
#P = np.diag(np.diag(M.A))
#print P.shape
#print error_value.shape
#error_value_pond = np.matrix(P * error_value)
#print b_angular[self._mask,0]
#print L
#L -= 10.
#wXc = SE3(eye(3),self.robot.position(q,1).inverse()*self.robot.com(q))
#Jang = wXc.action.T[3:,:]*self.robot.mass(q)[:6,:]
#b_com = wXc.action.T[3:,:]*b[:6]
#b_angular = -0*b_com
#bang = Jang*v
#return L[self._mask], 0., b_angular[self._mask,0]
return self._coeff * L_error[self._mask], 0., self._coeff * acc[
self._mask, 0]
def sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb,
z):
it = 0
success = False
n = [0, 0, 1]
vz = np.matrix(n).T
while not success and it < 10000:
it += 1
# sample a random position for movingLimb and try to project s0 to this position
qr = fullBody.shootRandomConfig()
q1 = s0.q()[::]
q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[
limb_ids[movingLimb][0]:limb_ids[movingLimb][1]]
s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact)
fullBody.setCurrentConfig(s1.q())
p = fullBody.getJointPosition(
fullBody.dict_limb_joint[movingLimb])[0:3]
p[0] = random.uniform(eff_x_range[0], eff_x_range[1])
p[1] = random.uniform(eff_y_range[0], eff_y_range[1])
p[2] = z
s1, success = StateHelper.addNewContact(s1, movingLimb, p, n)
if success:
"""
# force root orientation : (align current z axis with vertical)
quat_1 = Quaternion(s1.q()[6],s1.q()[3],s1.q()[4],s1.q()[5])
v_1 = quat_1.matrix() * vz
align = Quaternion.FromTwoVectors(v_1,vz)
rot = align * quat_1
q_root = s1.q()[0:7]
q_root[3:7] = rot.coeffs().T.tolist()[0]
"""
root = SE3.Identity()
root.translation = np.matrix(s1.q()[0:3]).T
root = sampleRotationAlongZ(root)
success = s1.projectToRoot(se3ToXYZQUATtuple(root))
# check if new state is in static equilibrium
if success:
# check stability
success = fullBody.isStateBalanced(s1.sId, 3)
if success:
success = projectInKinConstraints(fullBody, s1)
# check if transition is feasible according to CROC
if success:
#success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0)
success = fullBody.isReachableFromState(s0.sId, s1.sId)
return success, s1
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
# depend if eigenpy.switchToNumpyArray() have been called, FIXME : there should be a better way to check this
if len(rot[:, 0].shape) == 1:
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)
else:
rot[:, 0] = vect[3:6]
rot[:, 1] = vect[6:9]
rot[:, 2] = vect[9:12]
placement.rotation = rot
return placement
