def getFrameTrajectoryFromSolver(self, solver): if len(self.frameTrajNames) == 0: return None ps = {fr: [] for fr in self.frameTrajNames} models = solver.problem.runningModels.tolist() + [ solver.problem.terminalModel ] datas = solver.problem.runningDatas.tolist() + [ solver.problem.terminalData ] for key, p in ps.items(): frameId = int(key) for i, data in enumerate(datas): model = models[i] if hasattr(data, "differential"): # Update the frame placement if there is not contact. # Note that, in non-contact cases, the action model does not compute it for efficiency reason if len(data.differential.multibody.contacts.contacts. todict().items()) == 0: pinocchio.updateFramePlacement( model.differential.state.pinocchio, data.differential.multibody.pinocchio, frameId) pose = data.differential.multibody.pinocchio.oMf[frameId] p.append( np.asarray(pose.translation.T).reshape(-1).tolist()) elif isinstance( data, libcrocoddyl_pywrap.ActionDataImpulseFwdDynamics): pose = data.multibody.pinocchio.oMf[frameId] p.append( np.asarray(pose.translation.T).reshape(-1).tolist()) return ps
def get_link_iso(self, link_id): ret = np.eye(4) frame_id = self._model.getFrameId(link_id) trans = pin.updateFramePlacement(self._model, self._data, frame_id) ret[0:3, 0:3] = trans.rotation ret[0:3, 3] = trans.translation return np.copy(ret)
def BaseVelocityFromKinAndIMU(self, contactFrameId): """Estimate the velocity of the base with forward kinematics using a contact point that is supposed immobile in world frame Args: contactFrameId (int): ID of the contact point frame (foot frame) """ frameVelocity = pin.getFrameVelocity(self.model, self.data, contactFrameId, pin.ReferenceFrame.LOCAL) framePlacement = pin.updateFramePlacement(self.model, self.data, contactFrameId) # Angular velocity of the base wrt the world in the base frame (Gyroscope) _1w01 = self.IMU_ang_vel.reshape((3, 1)) # Linear velocity of the foot wrt the base in the foot frame _Fv1F = frameVelocity.linear # Level arm between the base and the foot _1F = np.array(framePlacement.translation) # Orientation of the foot wrt the base _1RF = framePlacement.rotation # Linear velocity of the base wrt world in the base frame _1v01 = self.cross3(_1F.ravel(), _1w01.ravel()) - \ (_1RF @ _Fv1F.reshape((3, 1))) # IMU and base frames have the same orientation # _iv0i = _1v01 + self.cross3(self._1Mi.translation.ravel(), _1w01.ravel()) return np.array(_1v01)
def calcDiff3d(self, q): pin.forwardKinematics(robot.model, robot.data, q) M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex) pin.computeJointJacobians(robot.model, robot.data, q) J = pin.getFrameJacobian(robot.model, robot.data, self.frameIndex, pin.LOCAL_WORLD_ALIGNED)[:3, :] return 2 * J.T @ (M.translation - self.Mtarget.translation)
def callback(self, q): print(1) q = np.matrix(q).T pin.forwardKinematics(robot.model, robot.data, q) M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex) gv.applyConfiguration('world/blue', pin.SE3ToXYZQUATtuple(M)) gv.applyConfiguration('world/box', pin.SE3ToXYZQUATtuple(self.Mtarget)) robot.display(q) time.sleep(1e-2)
def test_getters(self): data = self.model.createData() q = pin.randomConfiguration(self.model) v = np.random.rand(self.model.nv) a = np.random.rand(self.model.nv) pin.forwardKinematics(self.model, data, q, v, a) T = pin.updateFramePlacement(self.model, data, self.frame_idx) self.assertApprox(T, data.oMi[self.parent_idx].act(self.frame_placement)) v = pin.getFrameVelocity(self.model, data, self.frame_idx) self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) self.assertApprox( v, data.oMi[self.parent_idx].act(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) self.assertApprox( v, pin.SE3(T.rotation, np.zeros(3)).act( self.frame_placement.actInv(data.v[self.parent_idx]))) a = pin.getFrameAcceleration(self.model, data, self.frame_idx) self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) self.assertApprox( a, data.oMi[self.parent_idx].act(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) self.assertApprox( a, pin.SE3(T.rotation, np.zeros(3)).act( self.frame_placement.actInv(data.a[self.parent_idx]))) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) a = pin.getFrameClassicalAcceleration( self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
def createEESplines(self, rmodel, rdata, xs, t_sampling=0.005): N = len(xs) abscissa = a2m(np.linspace(0., t_sampling * (N - 1), N)) self.ee_splines = EESplines() for patch in self.ee_map.keys(): p = np.zeros((3, N)) m = np.zeros((3, N)) for i in range(N): q = a2m(xs[i][:rmodel.nq]) v = a2m(xs[i][-rmodel.nv:]) pinocchio.forwardKinematics(rmodel, rdata, q, v) p[:, i] = m2a( pinocchio.updateFramePlacement(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).translation) m[:, i] = m2a(pinocchio.getFrameVelocity(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).linear) self.ee_splines.update([[patch, CubicHermiteSpline(abscissa, p, m)]]) return
def ForwardKinematics(self, q, v=np.zeros(3)): """ Description: 1. Computes the Cartesian Position & Velocity of the robot 2. The TCP position is w.r.t. the 'tip_link' 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,) : Cartesian Position of the robot. np.array (3,) : Cartesian Velocity of the robot. """ index = self.EOAT_ID frame = pin.ReferenceFrame.LOCAL pin.forwardKinematics(self.model, self.model_data, q, v) pin.updateFramePlacements(self.model, self.model_data) pos = pin.updateFramePlacement(self.model, self.model_data, index) vel = pin.getFrameVelocity(self.model, self.model_data, index, frame) return np.array(pos)[:3, -1], np.array(vel)[:3]
def writeToMessage(self, t, q, v=None, tau=None, p=dict(), pd=dict(), f=dict(), s=dict()): # Filling the time information self._msg.header.stamp = rospy.Time(t) self._msg.time = t if v is None: v = np.zeros(self._model.nv) if tau is None: tau = np.zeros(self._model.njoints - 2) # Filling the centroidal state pinocchio.centerOfMass(self._model, self._data, q, v) c = self._data.com[0] cd = self._data.vcom[0] # Center of mass self._msg.centroidal.com_position.x = c[0] self._msg.centroidal.com_position.y = c[1] self._msg.centroidal.com_position.z = c[2] self._msg.centroidal.com_velocity.x = cd[0] self._msg.centroidal.com_velocity.y = cd[1] self._msg.centroidal.com_velocity.z = cd[2] # Base self._msg.centroidal.base_orientation.x = q[3] self._msg.centroidal.base_orientation.y = q[4] self._msg.centroidal.base_orientation.z = q[5] self._msg.centroidal.base_orientation.w = q[6] self._msg.centroidal.base_angular_velocity.x = v[3] self._msg.centroidal.base_angular_velocity.y = v[4] self._msg.centroidal.base_angular_velocity.z = v[5] # Momenta momenta = pinocchio.computeCentroidalMomentum(self._model, self._data) momenta_rate = pinocchio.computeCentroidalMomentumTimeVariation( self._model, self._data) self._msg.centroidal.momenta.linear.x = momenta.linear[0] self._msg.centroidal.momenta.linear.y = momenta.linear[1] self._msg.centroidal.momenta.linear.z = momenta.linear[2] self._msg.centroidal.momenta.angular.x = momenta.angular[0] self._msg.centroidal.momenta.angular.y = momenta.angular[1] self._msg.centroidal.momenta.angular.z = momenta.angular[2] self._msg.centroidal.momenta_rate.linear.x = momenta_rate.linear[0] self._msg.centroidal.momenta_rate.linear.y = momenta_rate.linear[1] self._msg.centroidal.momenta_rate.linear.z = momenta_rate.linear[2] self._msg.centroidal.momenta_rate.angular.x = momenta_rate.angular[0] self._msg.centroidal.momenta_rate.angular.y = momenta_rate.angular[1] self._msg.centroidal.momenta_rate.angular.z = momenta_rate.angular[2] # Filling the joint state njoints = self._model.njoints - 2 for j in range(njoints): self._msg.joints[j].position = q[7 + j] self._msg.joints[j].velocity = v[6 + j] self._msg.joints[j].effort = tau[j] # Filling the contact state names = list(p.keys()) + list(pd.keys()) + list(f.keys()) + list( s.keys()) names = list(dict.fromkeys(names)) self._msg.contacts = [None] * len(names) if len(names) != 0 and (len(p.keys()) == 0 or len(pd.keys()) == 0): pinocchio.forwardKinematics(self._model, self._data, q, v) for i, name in enumerate(names): contact_msg = ContactState() contact_msg.name = name frame_id = self._model.getFrameId(name) # Retrive the contact position if name in p: pose = pinocchio.SE3ToXYZQUAT(p[name]) else: oMf = pinocchio.updateFramePlacement(self._model, self._data, frame_id) pose = pinocchio.SE3ToXYZQUAT(oMf) # Retrieve the contact velocity if name in pd: ovf = pd[name] else: ovf = pinocchio.getFrameVelocity( self._model, self._data, frame_id, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED) # Storing the contact position and velocity inside the message contact_msg.pose.position.x = pose[0] contact_msg.pose.position.y = pose[1] contact_msg.pose.position.z = pose[2] contact_msg.pose.orientation.x = pose[3] contact_msg.pose.orientation.y = pose[4] contact_msg.pose.orientation.z = pose[5] contact_msg.pose.orientation.w = pose[6] contact_msg.velocity.linear.x = ovf.linear[0] contact_msg.velocity.linear.y = ovf.linear[1] contact_msg.velocity.linear.z = ovf.linear[2] contact_msg.velocity.angular.x = ovf.angular[0] contact_msg.velocity.angular.y = ovf.angular[1] contact_msg.velocity.angular.z = ovf.angular[2] # Retrieving and storing force data if name in f: contact_info = f[name] ctype = contact_info[0] force = contact_info[1] contact_msg.type = ctype contact_msg.wrench.force.x = force.linear[0] contact_msg.wrench.force.y = force.linear[1] contact_msg.wrench.force.z = force.linear[2] contact_msg.wrench.torque.x = force.angular[0] contact_msg.wrench.torque.y = force.angular[1] contact_msg.wrench.torque.z = force.angular[2] if name in s: terrain_info = s[name] norm = terrain_info[0] friction = terrain_info[1] contact_msg.surface_normal.x = norm[0] contact_msg.surface_normal.y = norm[1] contact_msg.surface_normal.z = norm[2] contact_msg.friction_coefficient = friction self._msg.contacts[i] = contact_msg return copy.deepcopy(self._msg)
def residual(self, q): '''Compute score from a configuration''' pin.forwardKinematics(self.rmodel, self.rdata, q) M = pin.updateFramePlacement(self.rmodel, self.rdata, self.frameIndex) self.deltaM = self.Mtarget.inverse() * M return pin.log(self.deltaM).vector
def update_frame(robot, data, name): frame_id = robot.getFrameId(name) pin.updateFramePlacement(robot, data, frame_id)
def update_frame(model, data, name): frame_id = model.getFrameId(name) pnc.updateFramePlacement(model, data, frame_id)
def run_filter(self, k, gait, device, goals): """Run the complementary filter to get the filtered quantities Args: k (int): Number of inv dynamics iterations since the start of the simulation gait (4xN array): Contact state of feet (gait matrix) device (object): Interface with the masterboard or the simulation goals (3x4 array): Target locations of feet on the ground """ feet_status = gait[0, :].copy() # Current contact state of feet remaining_steps = 1 # Remaining MPC steps for the current gait phase while (np.array_equal(feet_status, gait[remaining_steps, :])): remaining_steps += 1 # Update IMU data self.get_data_IMU(device) # Angular position of the trunk self.filt_ang_pos[:] = self.IMU_ang_pos # Angular velocity of the trunk self.filt_ang_vel[:] = self.IMU_ang_vel # Update joints data self.get_data_joints(device) # Update nb of iterations since contact self.k_since_contact += feet_status # Increment feet in stance phase self.k_since_contact *= feet_status # Reset feet in swing phase # Update forward kinematics data self.get_data_FK(feet_status) # Update forward geometry data self.get_xyz_feet(feet_status, goals) # Tune alpha depending on the state of the gait (close to contact switch or not) a = np.ceil(np.max(self.k_since_contact) / 10) - 1 b = remaining_steps n = 1 # Nb of steps of margin around contact switch v_max = 1.00 v_min = 0.97 # Minimum alpha value c = ((a + b) - 2 * n) * 0.5 if (a <= (n - 1)) or (b <= n): # If we are close from contact switch self.alpha = v_max # Only trust IMU data self.close_from_contact = True # Raise flag else: self.alpha = v_min + (v_max - v_min) * np.abs(c - (a - n)) / c #self.alpha = 0.997 self.close_from_contact = False # Lower flag if not self.kf_enabled: # Use cascade of complementary filters # Rotation matrix to go from base frame to world frame oRb = pin.Quaternion(np.array([self.IMU_ang_pos ]).T).toRotationMatrix() """self.debug_o_lin_vel += 0.002 * (oRb @ np.array([self.IMU_lin_acc]).T) # TOREMOVE self.filt_lin_vel[:] = (oRb.T @ self.debug_o_lin_vel).ravel()""" # Get FK estimated velocity at IMU location (base frame) cross_product = self.cross3(self._1Mi.translation.ravel(), self.IMU_ang_vel).ravel() i_FK_lin_vel = self.FK_lin_vel[:] + cross_product # Get FK estimated velocity at IMU location (world frame) oi_FK_lin_vel = (oRb @ np.array([i_FK_lin_vel]).T).ravel() # Integration of IMU acc at IMU location (world frame) oi_filt_lin_vel = self.filter_xyz_vel.compute( oi_FK_lin_vel, (oRb @ np.array([self.IMU_lin_acc]).T).ravel(), alpha=self.alpha) # Filtered estimated velocity at IMU location (base frame) i_filt_lin_vel = (oRb.T @ np.array([oi_filt_lin_vel]).T).ravel() # Filtered estimated velocity at center base (base frame) b_filt_lin_vel = i_filt_lin_vel - cross_product # Filtered estimated velocity at center base (world frame) ob_filt_lin_vel = (oRb @ np.array([b_filt_lin_vel]).T).ravel() # Position of the center of the base from FGeometry and filtered velocity (world frame) self.filt_lin_pos[:] = self.filter_xyz_pos.compute( self.FK_xyz[:] + self.xyz_mean_feet[:], ob_filt_lin_vel, alpha=np.array([0.995, 0.995, 0.9])) # Velocity of the center of the base (base frame) self.filt_lin_vel[:] = b_filt_lin_vel else: # Use Kalman filter # Rotation matrix to go from base frame to world frame oRb = pin.Quaternion(np.array([self.IMU_ang_pos ]).T).toRotationMatrix() # Update coefficients depending on feet status self.kf.updateCoeffs(feet_status) # Prediction step of the Kalman filter with IMU acceleration self.kf.predict(oRb @ self.IMU_lin_acc.reshape((3, 1))) # Get position of IMU relative to feet in base frame for i in range(4): framePlacement = -pin.updateFramePlacement( self.model, self.data, self.indexes[i]).translation self.Z[(3 * i):(3 * (i + 1)), 0:1] = oRb @ (framePlacement + self._1Mi.translation.ravel()).reshape( (3, 1)) self.Z[ 12 + i, 0] = 0.0 # (oRb @ framePlacement.reshape((3, 1)))[2, 0] + self.filt_lin_pos[2] # Correction step of the Kalman filter with position and velocity estimations by FK # self.Z[0:3, 0] = self.FK_xyz[:] + self.xyz_mean_feet[:] # self.Z[3:6, 0] = oRb.T @ self.FK_lin_vel self.kf.correct(self.Z) # Retrieve and store results cross_product = self.cross3(self._1Mi.translation.ravel(), self.IMU_ang_vel).ravel() self.filt_lin_pos[:] = self.kf.X[0:3, 0] - self._1Mi.translation.ravel( ) # base position in world frame self.filt_lin_vel[:] = oRb.transpose() @ ( self.kf.X[3:6, 0] - cross_product ) # base velocity in base frame # Logging self.log_alpha[self.k_log] = self.alpha self.feet_status[:] = feet_status # Save contact status sent to the estimator for logging self.feet_goals[:, :] = goals.copy( ) # Save feet goals sent to the estimator for logging self.log_IMU_lin_acc[:, self.k_log] = self.IMU_lin_acc[:] self.log_HP_lin_vel[:, self.k_log] = self.HP_lin_vel[:] self.log_LP_lin_vel[:, self.k_log] = self.LP_lin_vel[:] self.log_FK_lin_vel[:, self.k_log] = self.FK_lin_vel[:] self.log_filt_lin_vel[:, self.k_log] = self.filt_lin_vel[:] self.log_o_filt_lin_vel[:, self.k_log] = self.o_filt_lin_vel[:, 0] # Output filtered position vector (19 x 1) self.q_filt[0:3, 0] = self.filt_lin_pos if self.perfectEstimator: # Base height directly from PyBullet self.q_filt[2, 0] = device.dummyPos[2] - 0.0155 # Minus feet radius self.q_filt[3:7, 0] = self.filt_ang_pos self.q_filt[ 7:, 0] = self.actuators_pos # Actuators pos are already directly from PyBullet # Output filtered velocity vector (18 x 1) if self.perfectEstimator: # Linear velocities directly from PyBullet self.v_filt[0:3, 0] = (1 - self.alpha_v) * self.v_filt[ 0:3, 0] + self.alpha_v * device.b_baseVel else: self.v_filt[0:3, 0] = (1 - self.alpha_v) * self.v_filt[ 0:3, 0] + self.alpha_v * self.filt_lin_vel self.v_filt[ 3:6, 0] = self.filt_ang_vel # Angular velocities are already directly from PyBullet self.v_filt[ 6:, 0] = self.actuators_vel # Actuators velocities are already directly from PyBullet ### # Update model used for the forward kinematics """pin.forwardKinematics(self.model, self.data, self.q_filt, self.v_filt) pin.updateFramePlacements(self.model, self.data) z_min = 100 for i in (np.where(feet_status == 1))[0]: # Consider only feet in contact # Estimated position of the base using the considered foot framePlacement = pin.updateFramePlacement(self.model, self.data, self.indexes[i]) z_min = np.min((framePlacement.translation[2], z_min)) self.q_filt[2, 0] -= z_min""" ### # Output filtered actuators velocity for security checks self.v_secu[:] = ( 1 - self.alpha_secu ) * self.actuators_vel + self.alpha_secu * self.v_secu[:] # Increment iteration counter self.k_log += 1 return 0
def get_data_FK(self, feet_status): """Get data with forward kinematics and forward geometry (linear velocity, angular velocity and position) Args: feet_status (4x0 numpy array): Current contact state of feet """ # Update estimator FK model self.q_FK[7:, 0] = self.actuators_pos # Position of actuators self.v_FK[6:, 0] = self.actuators_vel # Velocity of actuators # Position and orientation of the base remain at 0 # Linear and angular velocities of the base remain at 0 # Update model used for the forward kinematics self.q_FK[3:7, 0] = np.array([0.0, 0.0, 0.0, 1.0]) pin.forwardKinematics(self.model, self.data, self.q_FK, self.v_FK) # pin.updateFramePlacements(self.model, self.data) # Update model used for the forward geometry self.q_FK[3:7, 0] = self.IMU_ang_pos[:] pin.forwardKinematics(self.model_for_xyz, self.data_for_xyz, self.q_FK) # Get estimated velocity from updated model cpt = 0 vel_est = np.zeros((3, )) xyz_est = np.zeros((3, )) for i in (np.where( feet_status == 1))[0]: # Consider only feet in contact if self.k_since_contact[ i] >= 16: # Security margin after the contact switch # Estimated velocity of the base using the considered foot vel_estimated_baseframe = self.BaseVelocityFromKinAndIMU( self.indexes[i]) # Estimated position of the base using the considered foot framePlacement = pin.updateFramePlacement( self.model_for_xyz, self.data_for_xyz, self.indexes[i]) xyz_estimated = -framePlacement.translation # Logging self.log_v_est[:, i, self.k_log] = vel_estimated_baseframe[0:3, 0] self.log_h_est[i, self.k_log] = xyz_estimated[2] # Increment counter and add estimated quantities to the storage variables cpt += 1 vel_est += vel_estimated_baseframe[:, 0] # Linear velocity xyz_est += xyz_estimated # Position """r_foot = 0.0155 # 31mm of diameter on meshlab if i <= 1: vel_est[0] += r_foot * (self.actuators_vel[1+3*i] - self.actuators_vel[2+3*i]) else: vel_est[0] += r_foot * (self.actuators_vel[1+3*i] + self.actuators_vel[2+3*i])""" # If at least one foot is in contact, we do the average of feet results if cpt > 0: self.FK_lin_vel = vel_est / cpt self.FK_xyz = xyz_est / cpt return 0
def residual3d(self, q): '''Compute score from a configuration''' pin.forwardKinematics(robot.model, robot.data, q) M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex) return (M.translation - self.Mtarget.translation)
model = pin.buildModelFromUrdf(urdf_file) data = model.createData() print(model) q = np.array([np.pi / 2., 0., 0.]) # q = np.zeros(3) qdot = np.ones(3) pin.forwardKinematics(model, data, q, qdot) ## Print Frame Names print([frame.name for frame in model.frames]) ## Calculate j2 placement j2_frame = model.getFrameId('j1') j2_translation = pin.updateFramePlacement(model, data, j2_frame) print("j2 translation") print(j2_translation) ## Calculate l2 placement l2_frame = model.getFrameId('l2') l2_translation = pin.updateFramePlacement(model, data, l2_frame) print("l2 translation") print(l2_translation) ## Calculate j2 jacobian pin.computeJointJacobians(model, data, q) j2_jacobian = pin.getFrameJacobian(model, data, j2_frame, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) print("j2 jacobian") print(j2_jacobian)