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 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 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 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 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, 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 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 __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 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 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 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 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]
class TrajectorySE3LinearInterp(RefTrajectory):
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 __call__(self, t):
return self.compute_for_normalized_time(t - self.t0)
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 set_transform(origin, a, b):
from pinocchio import rpy, Quaternion
length = np.linalg.norm(b - a)
z = (b - a) / length
R = Quaternion.FromTwoVectors(np.array([0, 0, 1]), z).matrix()
origin.attrib['xyz'] = " ".join([str(v) for v in ((a + b) / 2)])
origin.attrib['rpy'] = " ".join([str(v) for v in rpy.matrixToRpy(R)])
def set_random_configs(self):
"""
randomly sample initial and goal configuration :
"""
self.q_init[:3] = [0, 0, 0.465]
self.q_init[3:7] = [0, 0, 0, 1]
self.q_init[-6] = self.v_init
random.seed()
alpha = random.uniform(0., 2. * np.pi)
#alpha = 4.
print("Test on a circle, alpha = ", alpha)
self.q_goal = self.q_init[::]
self.q_goal[:3] = [
self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465
]
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T
quat = Quaternion.FromTwoVectors(vx, v_goal)
self.q_goal[3:7] = quat.coeffs().tolist()
# set final velocity to fix the orientation :
self.q_goal[-6] = self.v_goal * np.sin(alpha)
self.q_goal[-5] = -self.v_goal * np.cos(alpha)
self.v(self.q_init)
print("initial root position : ", self.q_init)
print("final root position : ", self.q_goal)
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
self.alpha = alpha
# write problem in files :
with open(self.status_filename, "w") as f:
f.write("q_init= " + str(self.q_init) + "\n")
f.write("q_goal= " + str(self.q_goal) + "\n")
def createPhaseFromRBPRMState(fb, stateId, t_init=-1):
"""
Create a multicontact_api ContactPhase object from an rbprm state
:param fb: an instance of rbprm.FullBody
:param stateId: the Id of the state
:param t_init: the initial time of the contact phase
:return: a multicontact_api.ContactPhase with the same contacts as the rbprm state
"""
q = fb.getConfigAtState(stateId)
limbs_contact = fb.getAllLimbsInContact(stateId)
cp = createPhaseFromConfig(fb, q, limbs_contact, t_init)
if fb.cType == "_3_DOF":
# update the contact normal from the data in fullbody
for limbId in limbs_contact:
eeName = fb.dict_limb_joint[limbId]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(
stateId, False, limbId)
normal = np.array(n)
print("New normal : ", normal)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]),
normal)
placement = cp.contactPatch(eeName).placement
placement.rotation = quat.matrix()
cp.contactPatch(eeName).placement = placement
print("New placement : ", normal)
return cp
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 __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 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 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 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 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 rotationFromNormal(n):
"""
return a rotation matrix corresponding to the given contact normal
:param n: the normal of the surface (as a numpy array)
:return: a rotation matrix
"""
z_up = np.array([0., 0., 1.])
q = Quaternion.FromTwoVectors(z_up, n)
return q.matrix()
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 contactPlacementFromRBPRMState(fb, stateId, eeName):
# get limbName from effector name :
fb.setCurrentConfig(fb.getConfigAtState(stateId))
placement = SE3FromConfig(fb.getJointPosition(eeName))
if fb.cType == "_3_DOF":
limbId = list(fb.dict_limb_joint.keys())[list(fb.dict_limb_joint.values()).index(eeName)]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(stateId, False, limbId)
normal = np.array(n)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal)
placement.rotation = quat.matrix()
return placement
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)
# force root orientation : (align current z axis with vertical)
if success:
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]
success = s1.projectToRoot(q_root)
# check if new state is in static equilibrium
if success:
# check stability
success = fullBody.isStateBalanced(s1.sId, 3)
# 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 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 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
