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_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