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 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 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 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 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 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 _lerp(p0, p1, t):
return (1 - t) * p0 + t * p1
def slerp(q0, q1, t):
assert (t >= 0 and t <= 1)
a = AngleAxis(q0.inverse() * q1)
return Quaternion(AngleAxis(a.angle * t, a.axis))
def nlerp(q0, q1, t):
q0 = q0.coeffs()
q1 = q1.coeffs()
l = _lerp(q0, q1, t)
l /= norm(l)
return Quaternion(l[3, 0], *l[:3].T.tolist()[0])
q0 = Quaternion(SE3.Random().rotation)
q1 = Quaternion(SE3.Random().rotation)
gv.applyConfiguration('world/box', [0, 0, 0] + q0.coeffs().T.tolist()[0])
time.sleep(.1)
for t in np.arange(0, 1, .01):
q = nlerp(q0, q1, t)
gv.applyConfiguration('world/box', [0, 0, 0] + q.coeffs().T.tolist()[0])
gv.refresh()
time.sleep(.01)
time.sleep(.1)
gv.applyConfiguration('world/box', [0, 0, 0] + q1.coeffs().T.tolist()[0])
def slerp(q0, q1, t):
assert (t >= 0 and t <= 1)
a = AngleAxis(q0.inverse() * q1)
return Quaternion(AngleAxis(a.angle * t, a.axis))
def nlerp(q0, q1, t):
q0 = q0.coeffs()
q1 = q1.coeffs()
lerp = _lerp(q0, q1, t)
lerp /= norm(lerp)
return Quaternion(lerp[3, 0], *lerp[:3].T.tolist()[0])
q0 = Quaternion(SE3.Random().rotation)
q1 = Quaternion(SE3.Random().rotation)
gv.applyConfiguration('world/box',
[0, 0, 0] + q0.coeffs().T.tolist()[0]) # noqa
sleep(.1)
for t in np.arange(0, 1, .01):
q = nlerp(q0, q1, t)
gv.applyConfiguration('world/box',
[0, 0, 0] + q.coeffs().T.tolist()[0]) # noqa
gv.refresh() # noqa
sleep(.01)
sleep(.1)
gv.applyConfiguration('world/box',
[0, 0, 0] + q1.coeffs().T.tolist()[0]) # noqa
