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 update_state(self, q, v): nf, ny = self.nf, self.ny se3.computeAllTerms(self.robot.model, self.robot.data, q, v) se3.framesForwardKinematics(self.robot.model, self.robot.data, q) # replace with updateFramePlacements(model, data) M = self.robot.data.M #(7,7) Mj_diag = np.matrix(np.diag(np.diag(M[ny:,ny:]))) Jl,Jr = self.robot.get_Jl_Jr_world(q, False) J = np.vstack([Jl[1:3],Jr[1:3]]) # (4, 7) Mlf, Mrf = self.robot.get_Mlf_Mrf(q, False) pyl, pzl = Mlf.translation[1:].A1 pyr, pzr = Mrf.translation[1:].A1 com, com_vel = self.robot.get_com_and_derivatives(q, v) cy, cz = com.A1 X_am = np.matrix([-(pzl-cz),+(pyl-cy),-(pzr-cz),+(pyr-cy)]) # X_com = np.hstack([np.eye(2)/M[0,0],np.eye(2)/M[0,0]]) X_com = np.hstack([np.eye(2),np.eye(2)]) self.X = np.vstack([X_com, X_am]) self.X_pinv = np.linalg.pinv(self.X) self.A[ ny:2*ny, 2*ny:2*ny+nf] = self.X self.H[ ny:2*ny, 2*ny:2*ny+nf] = self.X self.XT_Mb_inv = self.X.T * np.linalg.inv(M[:ny,:ny]) Minv = np.linalg.inv(M) Upsilon = J*Minv*J.T S = matlib.zeros((self.na, self.nv)) S[:,self.nv-self.na:] = matlib.eye(self.na) JSTpinv = np.linalg.pinv(J*S.T) A = J*Minv*S.T*Mj_diag*JSTpinv self.K_A = self.K*A self.K_Upsilon = self.K*Upsilon
def relativePlacement(rmodel, rdata, config, frameIndex1, frameIndex2): pio.framesForwardKinematics(rmodel, rdata, config) oMf1 = rdata.oMf[frameIndex1] oMf2 = rdata.oMf[frameIndex2] return np.linalg.inv(oMf1) @ oMf2
def _inverse_kinematics_step( self, frame_id: int, xdes: np.ndarray, q0: np.ndarray) -> typing.Tuple[np.ndarray, np.ndarray]: """Compute one IK iteration for a single finger.""" dt = 1.0e-1 pinocchio.computeJointJacobians( self.robot_model, self.data, q0, ) pinocchio.framesForwardKinematics( self.robot_model, self.data, q0, ) Ji = pinocchio.getFrameJacobian( self.robot_model, self.data, frame_id, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED, )[:3, :] xcurrent = self.data.oMf[frame_id].translation try: Jinv = np.linalg.inv(Ji) except Exception: Jinv = np.linalg.pinv(Ji) err = xdes - xcurrent dq = Jinv.dot(xdes - xcurrent) qnext = pinocchio.integrate(self.robot_model, q0, dt * dq) return qnext, err
def display(self, q, force): """Display the robot at configuration q in the viewer by placing all the bodies.""" gui = self.gui pin.framesForwardKinematics(self.model,self.data,q) if self.display_visuals: pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data) gui.applyConfigurations ( [ self.getViewerNodeName(visual,pin.GeometryType.VISUAL) for visual in self.visual_model.geometryObjects ], [ pin.SE3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ] ) for contactIndex, contactName in enumerate(self.contactNames): forceVector = np.zeros(3) if(self.type=="Biped"): for i in range(4): forceVector += force[:,4*contactIndex+i] elif(self.type=="Quadruped"): forceVector =force[:,contactIndex] forceNorm = np.linalg.norm(forceVector) if forceNorm<=1.e-3: forceVisiblity = "OFF" else: forceVisiblity = "ON" self.displayContact(contactName, forceVector, forceVisiblity) gui.refresh()
def calcDiff(self, q): pin.framesForwardKinematics(self.rmodel, self.rdata, q) pin.computeJointJacobians(self.rmodel, self.rdata, q) M = self.rdata.oMf[self.frameIndex] J = pin.getFrameJacobian(self.rmodel, self.rdata, self.frameIndex, pin.LOCAL_WORLD_ALIGNED)[:3, :] return 2 * J.T @ (M.translation - self.ptarget)
def getForwardKinematics(self, q=None): if q is None: q = self.current_j_pos if q.shape[0] == 7: q_tmp = q.copy() q = np.zeros(9) q[:7] = q_tmp pinocchio.framesForwardKinematics(self.model, self.data, q)
def callback(self, q): if self.viz is None: return pin.framesForwardKinematics(self.rmodel, self.rdata, q) M = self.rdata.oMf[self.frameIndex] vizutils.applyViewerConfiguration(self.viz, 'world/ball', pin.SE3ToXYZQUATtuple(M)) vizutils.applyViewerConfiguration(self.viz, 'world/box', self.ptarget.tolist() + [1, 0, 0, 0]) self.viz.display(q) time.sleep(1e-2)
def forward_kinematics(joint_positions): """compute the forward kinematics""" pinocchio.framesForwardKinematics(model, data, joint_positions) # get the position of the tip link pos = data.oMf[tip_link_id].translation # convert from np.matrix to a flat array (for easy printing) pos = np.asarray(pos).reshape(-1) return pos
def getJacobian(self, q=None): if q is None: q = self.current_j_pos if q.shape[0] == 7: q_tmp = q.copy() q = np.zeros(9) q[:7] = q_tmp pinocchio.computeJointJacobians(self.model, self.data, q) pinocchio.framesForwardKinematics(self.model, self.data, q) return pinocchio.getFrameJacobian(self.model, self.data, self.end_effector_frame_id, pinocchio.LOCAL_WORLD_ALIGNED)[:, :7]
def compute_closed_loop_transition_matrix(gains_array, robot, q, v, update=True): gains = Gains(gains_array) ny=3 H = matlib.zeros((3*nf+2*ny, 3*nf+2*ny)) H[:ny, ny:2*ny] = matlib.eye(ny) H_f = matlib.zeros((3*nf,3*nf)) H_f[ :nf, nf:2*nf] = matlib.eye(nf) H_f[nf:2*nf, 2*nf:3*nf] = matlib.eye(nf) H_f[2*nf:, 2*nf:3*nf] = -gains.kd_bar*matlib.eye(nf) if(update): se3.computeAllTerms(robot.model, robot.data, q, v) se3.framesForwardKinematics(robot.model, robot.data, q) M = robot.data.M #(7,7) Jl,Jr = robot.get_Jl_Jr_world(q, False) J = np.vstack([Jl[1:3],Jr[1:3]]) # (4, 7) Mlf, Mrf = robot.get_Mlf_Mrf(q, False) pyl, pzl = Mlf.translation[1:].A1 pyr, pzr = Mrf.translation[1:].A1 com, com_vel = robot.get_com_and_derivatives(q, v) cy, cz = com.A1 X_am = np.matrix([-(pzl-cz),+(pyl-cy),-(pzr-cz),+(pyr-cy)]) X_com = np.hstack([np.eye(2),np.eye(2)]) X = np.vstack([X_com, X_am]) X_pinv = np.linalg.pinv(X) Minv = np.linalg.inv(M) Upsilon = J*Minv*J.T S = matlib.zeros((na,nv)) S[:,nv-na:] = matlib.eye(na) JSTpinv = np.linalg.pinv(J*S.T) A = J*Minv*S.T*Mj_diag*JSTpinv # compute closed-loop transition matrix K1 = gains.kp_bar*K*A*gains.Kf K2 = K*Upsilon + gains.kp_bar*matlib.eye(nf) H_f[2*nf:, 1*nf:2*nf] = - K2 H_f[2*nf:, 0*nf:1*nf] = - K1 H[2*ny:, 2*ny:] = H_f H[ ny:2*ny, 2*ny:2*ny+nf] = X H[ -nf:, :ny] = -K1*X_pinv*gains.Kp_com H[ -nf:, ny:2*ny] = -K1*X_pinv*gains.Kd_com return H
def footInverseKinematicsFixedBase(self, foot_pos_des, frame_name): frame_id = self.model.getFrameId(frame_name) blockIdx = self.getBlockIndex(frame_name) anymal_q0 = np.vstack([-0.1, 0.7, -1., -0.1, -0.7, 1., 0.1, 0.7, -1., 0.1, -0.7, 1.]) q = anymal_q0 eps = 0.005 IT_MAX = 200 DT = 1e-1 err = np.zeros((3, 1)) e = np.zeros((3, 1)) i = 0 while True: pinocchio.forwardKinematics(self.model, self.data, q) pinocchio.framesForwardKinematics(self.model, self.data, q) foot_pos = self.data.oMf[frame_id].translation # err = np.hstack([err, (foot_pos - foot_pos_des)]) # e = err[:,-1] # print foot_pos_des[0], foot_pos[[0]], foot_pos[[0]] - foot_pos_des[0] # e = foot_pos - foot_pos_des e[0] = foot_pos[[0]] - foot_pos_des[0] e[1] = foot_pos[[1]] - foot_pos_des[1] e[2] = foot_pos[[2]] - foot_pos_des[2] J = pinocchio.frameJacobian(self.model, self.data, q, frame_id) J_lin = J[:3, :] if np.linalg.norm(e) < eps: # print("IK Convergence achieved!") IKsuccess = True break if i >= IT_MAX: print( "\n Warning: the iterative algorithm has not reached convergence to the desired precision. Error is: ", np.linalg.norm(e)) IKsuccess = False break # print J_lin v = - np.linalg.pinv(J_lin) * e q = pinocchio.integrate(self.model, q, v * DT) i += 1 # print i q_leg = q[blockIdx:blockIdx + 3] J_leg = J_lin[:, blockIdx:blockIdx + 3] return q_leg, J_leg, err, IKsuccess
def forward_kinematics( self, joint_positions: typing.List[np.ndarray], joint_velocities: typing.Optional[np.ndarray] = None, ) -> typing.Union[typing.List[np.ndarray], typing.Tuple[ typing.List[np.ndarray], typing.List[np.ndarray]], ]: """Compute end-effector positions (and velocities) for the given joint configuration. Args: joint_positions: Flat list of angular joint positions. joint_velocities: Optional. Flat list of angular joint velocities. Returns: If only joint positions are given: List of end-effector positions. Each position is given as an np.array with x,y,z positions. If joint positions and velocities are given: Tuple with (i) list of end-effector positions and (ii) list of end-effector velocities. Each position and velocity is given as an np.array with x,y,z components. """ pinocchio.framesForwardKinematics(self.robot_model, self.data, joint_positions) positions = [ np.asarray( self.data.oMf[link_id].translation).reshape(-1).tolist() for link_id in self.tip_link_ids ] if joint_velocities is None: return positions else: pinocchio.forwardKinematics(self.robot_model, self.data, joint_positions, joint_velocities) velocities = [] for link_id in self.tip_link_ids: local_to_world_transform = pinocchio.SE3.Identity() local_to_world_transform.rotation = self.data.oMf[ link_id].rotation v_local = pinocchio.getFrameVelocity(self.robot_model, self.data, link_id) v_world = local_to_world_transform.act(v_local) velocities.append(v_world.linear) return positions, velocities
def TimeDerivativeJacobian(self, q, v): """ Description: 1. Computes the Time Derivative Jacobian Tensor of the robot. Args: q -> np.array (3,) : Joint Positions of the robot. v -> np.array (3,) : Joint Velocities of the robot. Returns: np.array (3,3): Time Derivative Jacobian Tensor of the robot. """ frame = pin.ReferenceFrame.LOCAL_WORLD_ALIGNED pin.computeJointJacobians(self.model, self.model_data, q) pin.framesForwardKinematics(self.model, self.model_data, q) pin.computeJointJacobiansTimeVariation(self.model, self.model_data, q, v) return pin.getJointJacobianTimeVariation(self.model, self.model_data, self.EOAT_ID, frame)[:3]
def check_offset_to_pinocchio_model_in_cart_space(self, q): import pinocchio from classic_framework.utils.geometric_transformation import mat2quat obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf") model = pinocchio.buildModelFromUrdf(obj_urdf) data = model.createData() q_pin = np.zeros(9) q_pin[:7] = q pinocchio.framesForwardKinematics(model, data, q_pin) pinocchio.computeJointJacobians(model, data, q_pin) pinocchio.framesForwardKinematics(model, data, q_pin) # get orientation rotation_matrix = data.oMf[model.getFrameId('panda_grasptarget')].rotation quaternions = mat2quat(rotation_matrix) # [w x y z] jac = pinocchio.getFrameJacobian(model, data, model.getFrameId('panda_grasptarget'), pinocchio.LOCAL_WORLD_ALIGNED)[:, :7] print('position offset:', self.current_c_pos - np.array(data.oMf[model.getFrameId('panda_grasptarget')].translation))
def forward_kinematics(self, joint_positions): """ Compute end effector positions for the given joint configuration. Args: finger (SimFinger): a SimFinger object joint_positions (list): Flat list of angular joint positions. Returns: List of end-effector positions. Each position is given as an np.array with x,y,z positions. """ pinocchio.framesForwardKinematics( self.robot_model, self.data, joint_positions, ) return [ np.asarray(self.data.oMf[link_id].translation).reshape(-1).tolist() for link_id in self.tip_link_ids ]
def inverse_kinematics(self, fid, xdes, q0): """ Method not in use right now, but is here with the intention of using pinocchio for inverse kinematics instead of using the in-house IK solver of pybullet. """ raise NotImplementedError() dt = 1.0e-3 pinocchio.computeJointJacobians( self.robot_model, self.data, q0, ) pinocchio.framesKinematics( self.robot_model, self.data, q0, ) pinocchio.framesForwardKinematics( self.robot_model, self.data, q0, ) Ji = pinocchio.getFrameJacobian( self.robot_model, self.data, fid, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED, )[:3, :] xcurrent = self.data.oMf[fid].translation try: Jinv = np.linalg.inv(Ji) except Exception: Jinv = np.linalg.pinv(Ji) dq = Jinv.dot(xdes - xcurrent) qnext = pinocchio.integrate(self.robot_model, q0, dt * dq) return qnext
def compute_freeflyer_state_from_fixed_body(robot, position, velocity=None, acceleration=None, fixed_body_name=None, ground_profile=None, use_theoretical_model=True): """ @brief Fill rootjoint data from articular data when a body is fixed parallel to world. @details If 'fixed_body_name' is omitted, it will default to the contact point that ensures no contact points are going through the ground and a single one is touching it. @remark 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 robot The jiminy robot @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 See position @param[inout] acceleration See position @param fixed_body_name The name of the body frame that is considered fixed parallel to world frame @param ground_profile The ground profile callback """ if not robot.has_freeflyer: raise ValueError("The robot does not have a freeflyer.") if use_theoretical_model: pnc_model = robot.pinocchio_model_th pnc_data = robot.pinocchio_data_th else: pnc_model = robot.pinocchio_model pnc_data = robot.pinocchio_data position[:6].fill(0.0) position[6] = 1.0 if velocity is not None and acceleration is not None: velocity[:6].fill(0.0) acceleration[:6].fill(0.0) pin.forwardKinematics(pnc_model, pnc_data, position, velocity, acceleration) elif velocity is not None: velocity[:6].fill(0.0) pin.forwardKinematics(pnc_model, pnc_data, position, velocity) else: pin.forwardKinematics(pnc_model, pnc_data, position) pin.framesForwardKinematics(pnc_model, pnc_data, position) if fixed_body_name is None: fixed_body_name = _compute_closest_contact_frame( robot, ground_profile, use_theoretical_model) ff_M_fixed_body = get_body_world_transform( robot, fixed_body_name, use_theoretical_model, copy=False) if ground_profile is not None: ground_translation = np.zeros(3) ground_translation[2], normal = ground_profile(ff_M_fixed_body.translation) ground_rotation = pin.Quaternion.FromTwoVectors(np.array([0.0, 0.0, 1.0]), normal).matrix() w_M_ground = pin.SE3(ground_rotation, ground_translation) else: w_M_ground = pin.SE3.Identity() w_M_ff = w_M_ground.act(ff_M_fixed_body.inverse()) position[:7] = pin.se3ToXYZQUAT(w_M_ff) if velocity is not None: ff_v_fixed_body = get_body_world_velocity( robot, fixed_body_name, use_theoretical_model) base_link_velocity = - ff_v_fixed_body velocity[:6] = base_link_velocity.vector if acceleration is not None: ff_a_fixedBody = get_body_world_acceleration( robot, fixed_body_name, use_theoretical_model) base_link_acceleration = - ff_a_fixedBody acceleration[:6] = base_link_acceleration.vector return fixed_body_name
def update_quantities(jiminy_model, position, velocity=None, acceleration=None, update_physics=True, update_com=False, update_energy=False, update_jacobian=False, use_theoretical_model=True): """ @brief Compute all quantities using position, velocity and acceleration configurations. @details Run multiple algorithms to compute all quantities which can be known with the model position, velocity and acceleration configuration. This includes: - body spatial transforms, - body spatial velocities, - body spatial drifts, - body transform acceleration, - body transform jacobians, - center-of-mass position, - center-of-mass velocity, - center-of-mass drift, - center-of-mass acceleration, (- center-of-mass jacobian : No Python binding available so far), - articular inertia matrix, - non-linear effects (Coriolis + gravity) Computation results are stored in internal data and can be retrieved with associated getters. @note This function modifies internal data. @param jiminy_model The Jiminy model @param position Joint position vector @param velocity Joint velocity vector @param acceleration Joint acceleration vector @pre model_.nq == positionIn.size() @pre model_.nv == velocityIn.size() @pre model_.nv == accelerationIn.size() """ 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 if (update_physics and update_com and \ update_energy and update_jacobian and \ velocity is not None): pnc.computeAllTerms(pnc_model, pnc_data, position, velocity) else: if update_physics: if velocity is not None: pnc.nonLinearEffects(pnc_model, pnc_data, position, velocity) pnc.crba(pnc_model, pnc_data, position) if update_jacobian: # if update_com: # pnc.getJacobianComFromCrba(pnc_model, pnc_data) pnc.computeJointJacobians(pnc_model, pnc_data) if update_com: if velocity is None: pnc.centerOfMass(pnc_model, pnc_data, position) elif acceleration is None: pnc.centerOfMass(pnc_model, pnc_data, position, velocity) else: pnc.centerOfMass(pnc_model, pnc_data, position, velocity, acceleration) else: if velocity is None: pnc.forwardKinematics(pnc_model, pnc_data, position) elif acceleration is None: pnc.forwardKinematics(pnc_model, pnc_data, position, velocity) else: pnc.forwardKinematics(pnc_model, pnc_data, position, velocity, acceleration) pnc.framesForwardKinematics(pnc_model, pnc_data, position) if update_energy: if velocity is not None: pnc.kineticEnergy(pnc_model, pnc_data, position, velocity, False) pnc.potentialEnergy(pnc_model, pnc_data, position, False)
urdf_pkg_path = get_package_share_directory("robot_properties_manipulator") urdf_path = os.path.join(urdf_pkg_path, "urdf", "finger.urdf") model = pinocchio.buildModelFromUrdf(urdf_path) data = model.createData() tip_link_id = model.getFrameId("finger_tip_link") robot = robot_fingers.Robot( robot_interfaces.finger, robot_fingers.create_real_finger_backend, "finger.yml", ) robot.initialize() action = robot.Action() while True: t = robot.frontend.append_desired_action(action) joint_positions = robot.frontend.get_observation(t).position # compute the forward kinematics pinocchio.framesForwardKinematics(model, data, joint_positions) # get the position of the tip link pos = data.oMf[tip_link_id].translation # convert from np.matrix to a flat array (for easy printing) pos = np.asarray(pos).reshape(-1) n_joints = len(pos) format_string = "\r" + ", ".join(["{: 6.3f}"] * n_joints) print(format_string.format(*pos), end="")
# --- Test vs CoM print() print("--- Test vs CoM ---") estimator = SimpleZmpEstimator("ciao2") model = pin.buildSampleModelHumanoid() data = model.createData() rightId = model.getFrameId('rleg_effector_body') leftId = model.getFrameId('lleg_effector_body') q = pin.neutral(model) q[:3] = np.matrix([1.0, 0.0, 0.0]).T # displace freeflyer along x for testing q[3:7] = np.matrix([np.sqrt(2) / 2, 0.0, np.sqrt(2) / 2, 0.0]).T # orient the base so that the feet are flat pin.framesForwardKinematics(model, data, q) poseRight = data.oMf[rightId].homogeneous.tolist() poseLeft = data.oMf[leftId].homogeneous.tolist() com = pin.centerOfMass(model, data, q).flatten().tolist()[0] m = sum([inertia.mass for inertia in model.inertias[1:]]) g = 9.81 fz = m * g / 2.0 forceLeft = [0.0, 0.0, fz] forceRight = [0.0, 0.0, fz] lever = com[0] - data.oMf[rightId].translation[0] tauy = -fz * lever wrenchLeft = forceLeft + [0.0, tauy, 0.0] wrenchRight = forceRight + [0.0, tauy, 0.0]
# Getting the frame index of each foot ID_FL = solo.model.getFrameId("FL_FOOT") ID_FR = solo.model.getFrameId("FR_FOOT") ID_HL = solo.model.getFrameId("HL_FOOT") ID_HR = solo.model.getFrameId("HR_FOOT") dt = 0.01 #Integration step #q[8]=np.pi/2 #q[7]=0 hist_err = [] #To see the error convergence for i in range(2000): pin.forwardKinematics(solo.model, solo.data, q) pin.framesForwardKinematics(solo.model, solo.data, q) #useless ? pin.updateFramePlacements(solo.model, solo.data) # Getting the current height (on axis z) of each foot hFL = solo.data.oMf[ID_FL].translation[2] hFR = solo.data.oMf[ID_FR].translation[2] hHL = solo.data.oMf[ID_HL].translation[2] hHR = solo.data.oMf[ID_HR].translation[2] # Computing the error in the world frame err_FL = np.concatenate((np.zeros([2, 1]), hFL)) err_FR = np.concatenate((np.zeros([2, 1]), hFR)) err_HL = np.concatenate((np.zeros([2, 1]), hHL)) err_HR = np.concatenate((np.zeros([2, 1]), hHR)) # Computing the error in the local frame
def update(self, solo, qmes12, vmes12): """Update the quantities of the Interface based on the last measurements from the simulation Args: solo (object): Pinocchio wrapper for the quadruped qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators vmes12 (18x1 array): the linear/angular velocity of the trunk and angular velocity of actuators """ # Rotation matrix from the world frame to the base frame self.oRb = pin.Quaternion(qmes12[3:7]).matrix() # Linear and angular velocity in base frame self.vmes12_base = vmes12.copy() self.vmes12_base[0:3, 0:1] = self.oRb.transpose() @ self.vmes12_base[0:3, 0:1] self.vmes12_base[3:6, 0:1] = self.oRb.transpose() @ self.vmes12_base[3:6, 0:1] """# Update Kinematics (required or automatically done by other functions?) pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base) self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0] for i in self.indexes[1:]: self.mean_feet_z = np.min((self.mean_feet_z, solo.data.oMf[i].translation[2, 0])) qmes12[2, 0] -= self.mean_feet_z""" # Update Kinematics (required or automatically done by other functions?) pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base) pin.framesForwardKinematics(solo.model, solo.data, qmes12) # Get center of mass from Pinocchio pin.centerOfMass(solo.model, solo.data, qmes12, self.vmes12_base) # Update position/orientation of frames pin.updateFramePlacements(solo.model, solo.data) pin.ccrba(solo.model, solo.data, qmes12, self.vmes12_base) # Update minimum height of feet # TODO: Rename mean_feet_z into min_feet_z self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0] """for i in self.indexes: self.mean_feet_z += solo.data.oMf[i].translation[2, 0] self.mean_feet_z *= 0.25""" for i in self.indexes[1:]: self.mean_feet_z = np.min( (self.mean_feet_z, solo.data.oMf[i].translation[2, 0])) self.mean_feet_z = 0.0 # Store position, linear velocity and angular velocity in global frame self.oC = solo.data.com[0] self.oV = solo.data.vcom[0] self.oW = vmes12[3:6] pin.crba(solo.model, solo.data, qmes12) # Get SE3 object from world frame to base frame self.oMb = pin.SE3(pin.Quaternion(qmes12[3:7]), self.oC) self.RPY = pin.rpy.matrixToRpy(self.oMb.rotation) # Get SE3 object from world frame to local frame self.oMl = pin.SE3( pin.utils.rotate('z', self.RPY[2, 0]), np.array([qmes12[0, 0], qmes12[1, 0], self.mean_feet_z])) # Get position, linear velocity and angular velocity in local frame self.lC = self.oMl.inverse() * self.oC self.lV = self.oMl.rotation.transpose() @ self.oV self.lW = self.oMl.rotation.transpose() @ self.oW # Pos, vel and acc of feet for i, j in enumerate(self.indexes): # Position of feet in local frame self.o_feet[:, i:(i + 1)] = solo.data.oMf[j].translation self.l_feet[:, i:( i + 1)] = self.oMl.inverse() * solo.data.oMf[j].translation # getFrameVelocity output is in the frame of the foot so a transform is required self.ov_feet[:, i:( i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameVelocity( solo.model, solo.data, j).vector[0:3, 0:1] self.lv_feet[:, i:( i + 1 )] = self.oMl.rotation.transpose() @ self.ov_feet[:, i:(i + 1)] # getFrameAcceleration output is in the frame of the foot so a transform is required self.oa_feet[:, i:( i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameAcceleration( solo.model, solo.data, j).vector[0:3, 0:1] self.la_feet[:, i:( i + 1 )] = self.oMl.rotation.transpose() @ self.oa_feet[:, i:(i + 1)] # Orientation of the base in local frame # Base and local frames have the same yaw orientation in world frame self.abg[0:2] = self.RPY[0:2] # Position of shoulders in world frame for i in range(4): self.o_shoulders[:, i:(i + 1)] = self.oMl * self.l_shoulders[:, i] return 0
e2q = EulerToQuat('e2q') plug(base_estimator.q, e2q.euler) e2q.quaternion.recompute(0) print(e2q.quaternion.value) print(len(e2q.quaternion.value)) q_est = np.matrix(e2q.quaternion.value).T # --- Raw q difference --- print("--- Raw q difference ---") q_diff = q_est - q print(q_diff.flatten().tolist()[0]) # --- Estimated feet --- print("--- Estimated feet ---") data2 = model.createData() pin.framesForwardKinematics(model, data2, q_est) print(data2.oMf[rightId]) print(data2.oMf[leftId]) # --- DCM estimator --- print("--- DCM estimator ---") dcm_estimator = DcmEstimator('dcm_estimator') dcm_estimator.init(dt, robot_name) plug(e2q.quaternion, dcm_estimator.q) plug(base_estimator.v, dcm_estimator.v) dcm_estimator.c.recompute(0) print(dcm_estimator.c.value) # --- Direct CoM --- print("--- Direct CoM ---")
def residual(self, q): pin.framesForwardKinematics(self.rmodel, self.rdata, q) M = self.rdata.oMf[self.frameIndex] return M.translation - self.ptarget
def updateGeometryPlacements(self, q): pinocchio.framesForwardKinematics(self.model, self.data, np.array(q)) pinocchio.updateGeometryPlacements(self.model, self.data, self.gmodel, self.gdata)
pio.Quaternion(-0.4, 0.02, -0.5, 0.7).normalized().matrix(), np.matrix([.2, -.4, .7]).T) gv.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, .2) gv.applyConfiguration('world/framegoal', list(pio.se3ToXYZQUAT(oMgoal).flat)) DT = 1e-2 # Integration step. q0 = np.matrix([[ 0., 0., 0., 1., 0.18, 1.37, -0.24, -0.98, 0.98, 0., 0., 0., 0., -0.13, 0., 0., 0., 0. ]]).T q = q0.copy() herr = [] # Log the value of the error between tool and goal. # Loop on an inverse kinematics for 200 iterations. for i in range(200): # Integrate over 2 second of robot life pio.framesForwardKinematics(robot.model, robot.data, q) # Compute frame placements oMtool = robot.data.oMf[ IDX_TOOL] # Placement from world frame o to frame f oMtool oRtool = oMtool.rotation # Rotation from world axes to tool axes oRtool tool_Jtool = pio.computeFrameJacobian( robot.model, robot.data, q, IDX_TOOL) # 6D jacobian in local frame o_Jtool3 = oRtool * tool_Jtool[:3, :] # 3D jacobian in world frame o_TG = oMtool.translation - oMgoal.translation # vector from tool to goal, in world frame vq = -pinv(o_Jtool3) * o_TG q = pio.integrate(robot.model, q, vq * DT) robot.display(q) time.sleep(1e-3) herr.append(o_TG)
def _apply_fk(self, q): pin.framesForwardKinematics(self.wrapper.model, self.wrapper.data, q)
def get_oMf(self, q): pin.framesForwardKinematics(self.wrapper.model, self.dummy_data, q) return self.dummy_data.oMf
def compute_forward_kinematics(self, q): ''' Computes forward kinematics of all the frames and stores in data ''' pin.framesForwardKinematics(self.pin_robot.model, self.pin_robot.data, q)