def optimize_initial_position(self, init_state): # Optimize the initial configuration q = se3.neutral(self.robot.model) plan_joint_init_pos = self.planner_setting.get( PlannerVectorParam_KinematicDefaultJointPositions) if len(plan_joint_init_pos) != self.robot.num_ctrl_joints: raise ValueError( 'Number of joints in config file not same as required for robot\n' + 'Got %d joints but robot expects %d joints.' % (len(plan_joint_init_pos), self.robot.num_ctrl_joints)) q[7:] = np.matrix(plan_joint_init_pos).T q[2] = self.robot.floor_height + 0.32 #was 0.32 #print q[2] dq = np.matrix(np.zeros(self.robot.robot.nv)).T com_ref = init_state.com lmom_ref = np.zeros(3) amom_ref = np.zeros(3) endeff_pos_ref = np.array( [init_state.effPosition(i) for i in range(init_state.effNum())]) endeff_vel_ref = np.matrix(np.zeros((init_state.effNum(), 3))) endeff_contact = np.ones(init_state.effNum()) quad_goal = se3.Quaternion( se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T)) q[3:7] = quad_goal.coeffs() for iters in range(self.max_iterations): # Adding small P controller for the base orientation to always start with flat # oriented base. quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]), float(q[5])) amom_ref = 1e-1 * se3.log((quad_goal * quad_q.inverse()).matrix()) res = self.inv_kin.compute(q, dq, com_ref, lmom_ref, amom_ref, endeff_pos_ref, endeff_vel_ref, endeff_contact, None) q = se3.integrate(self.robot.model, q, res) if np.linalg.norm(res) < 1e-3: print('Found initial configuration after {} iterations'.format( iters + 1)) break if iters == self.max_iterations - 1: print('Failed to converge for initial setup.') print("initial configuration: \n", q) q[2] = 0.20 self.q_init = q.copy() self.dq_init = dq.copy()
def compute_contact_forces(self, qa, dqa, o_R_b, b_v_ob, b_omg_ob, _, tauj, world_frame=True): o_quat_b = pin.Quaternion(o_R_b).coeffs() q = np.concatenate( [np.array([0, 0, 0]), o_quat_b, qa]) # robot anywhere with orientation estimated from IMU vq = np.concatenate([b_v_ob, b_omg_ob, dqa]) # vq = np.hstack([np.zeros(3), b_omg_ob, dqa]) C = pin.computeCoriolisMatrix(self.rm, self.rd, q, vq) g = pin.computeGeneralizedGravity(self.rm, self.rd, q) M = pin.crba(self.rm, self.rd, q) p = M @ vq tauj = np.concatenate([np.zeros(6), tauj]) self.int = self.int + (tauj + C.T @ vq - g + self.r) * self.dt self.r = self.Ki * (p - self.int - self.p0) return taucf_to_oforces(self.robot, q, self.nv, o_R_b, self.r, self.contact_frame_id_lst)
def retrieve_pyb_data(self): """ Retrieve the position and orientation of the base in world frame as well as its linear and angular velocities """ # Joint states jointStates = pyb.getJointStates(self.robotId, self.bullet_joint_ids) self.qa = np.array([state[0] for state in jointStates]) self.va = np.array([state[1] for state in jointStates]) # Position and orientation of the trunk self.w_p_b, self.w_quat_b = pyb.getBasePositionAndOrientation( self.robotId) self.w_p_b, self.w_quat_b = np.array(self.w_p_b), np.array( self.w_quat_b) self.w_R_b = pin.Quaternion(self.w_quat_b.reshape( (4, 1))).toRotationMatrix() # Velocity of the trunk -> world coordinates given by pyb self.w_v_b, self.w_omg_b = pyb.getBaseVelocity(self.robotId) self.w_v_b, self.w_omg_b = np.array(self.w_v_b), np.array(self.w_omg_b) self.b_v_b, self.b_omg_b = self.w_R_b.T @ self.w_v_b, self.w_R_b.T @ self.w_omg_b # Joints configuration and velocity vector for free-flyer + 12 actuators self.q = np.hstack((self.w_p_b, self.w_quat_b, self.qa)) self.v = np.hstack((self.b_v_b, self.b_omg_b, self.va)) return 0
def log_state(self, logger, prefix): '''Log current state: the values logged are defined in CLAPTRAP_STATE_SUFFIXES @param logger Logger object @param prefix Prefix to add before each suffix. ''' rpy = se3.rpy.matrixToRpy( se3.Quaternion(self.q[6, 0], self.q[3, 0], self.q[4, 0], self.q[5, 0]).matrix()) logger.set(prefix + "roll", rpy[0, 0]) logger.set(prefix + "pitch", rpy[1, 0]) logger.set(prefix + "yaw", rpy[2, 0]) logger.set_vector(prefix + "omega", self.v[3:6, 0]) logger.set( prefix + "wheelVelocity", self.v[self.robot.model.joints[self.robot.model. getJointId("WheelJoint")].idx_v, 0]) se3.computeAllTerms(self.robot.model, self.robot.data, self.q, self.v) energy = self.robot.data.kinetic_energy + self.robot.data.potential_energy logger.set(prefix + "energy", energy) logger.set(prefix + "wheelTorque", self.tau[0, 0]) w_M_base = self.robot.framePosition( self.q, self.robot.model.getFrameId("Body"), False) logger.set_vector(prefix + "base", w_M_base.translation)
def pd_controller(t, q, v, q_ref, v_ref, a_ref): pitch = se3.rpy.matrixToRpy(se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix())[1, 0] dpitch = v[4, 0] u = - np.matrix([[Kp * (pitch + Kd * dpitch)]]) # -u: torque is applied to the wheel, so the opposite torque is applied to the base. return -u
def writeFromMessage(self, msg): t = msg.time q = np.zeros(self._model.nq) v = np.zeros(self._model.nv) tau = np.zeros(self._model.njoints - 2) p = dict() pd = dict() f = dict() s = dict() # Retrieve the generalized position and velocity, and joint torques q[3] = msg.centroidal.base_orientation.x q[4] = msg.centroidal.base_orientation.y q[5] = msg.centroidal.base_orientation.z q[6] = msg.centroidal.base_orientation.w v[3] = msg.centroidal.base_angular_velocity.x v[4] = msg.centroidal.base_angular_velocity.y v[5] = msg.centroidal.base_angular_velocity.z for j in range(len(msg.joints)): jointId = self._model.getJointId(msg.joints[j].name) - 2 q[jointId + 7] = msg.joints[j].position v[jointId + 6] = msg.joints[j].velocity tau[jointId] = msg.joints[j].effort pinocchio.centerOfMass(self._model, self._data, q, v) q[0] = msg.centroidal.com_position.x - self._data.com[0][0] q[1] = msg.centroidal.com_position.y - self._data.com[0][1] q[2] = msg.centroidal.com_position.z - self._data.com[0][2] v[0] = msg.centroidal.com_velocity.x - self._data.vcom[0][0] v[1] = msg.centroidal.com_velocity.y - self._data.vcom[0][1] v[2] = msg.centroidal.com_velocity.z - self._data.vcom[0][2] # Retrive the contact information for contact in msg.contacts: name = contact.name # Contact pose pose = contact.pose position = np.array( [pose.position.x, pose.position.y, pose.position.z]) quaternion = pinocchio.Quaternion(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z) p[name] = pinocchio.SE3(quaternion, position) # Contact velocity velocity = contact.velocity lin_vel = np.array( [velocity.linear.x, velocity.linear.y, velocity.linear.z]) ang_vel = np.array( [velocity.angular.x, velocity.angular.y, velocity.angular.z]) pd[name] = pinocchio.Motion(lin_vel, ang_vel) # Contact wrench wrench = contact.wrench force = np.array([wrench.force.x, wrench.force.y, wrench.force.z]) torque = np.array( [wrench.torque.x, wrench.torque.y, wrench.torque.z]) f[name] = [contact.type, pinocchio.Force(force, torque)] # Surface normal and friction coefficient normal = contact.surface_normal nsurf = np.array([normal.x, normal.y, normal.z]) s[name] = [nsurf, contact.friction_coefficient] return t, q, v, tau, p, pd, f, s
def transform_msg(transforms, Tmean, Tvar): return """Average transform from {} transforms: - translation: {} - rotation (x, y, z, w): {} - variance: {}""".format(len(transforms), Tmean.translation, pinocchio.Quaternion(Tmean.rotation).coeffs(), Tvar)
def compute_com_wrench(self, q, dq, des_pos, des_vel, des_ori, des_angvel): """Compute the desired COM wrench (equation 1). Args: des_pos: desired center of mass position at time t des_vel: desired center of mass velocity at time t des_ori: desired base orientation at time t (quaternions) des_angvel: desired base angular velocity at time t Returns: Computed w_com """ m = self.robot_mass robot = self.robot # com = np.reshape(np.array(q[0:3]), (3,)) com = pin.centerOfMass(robot.pin_robot.model, robot.pin_robot.data, q, dq) vcom = np.reshape(np.array(dq[0:3]), (3, )) Ib = robot.pin_robot.mass(q)[3:6, 3:6] quat_diff = self.quaternion_difference(arr(q[3:7]), arr(des_ori)) cur_angvel = arr(dq[3:6]) if self.rotate_vel_error: # Rotate the des and current angular velocity into the world frame. quat_des = pin.Quaternion(des_ori[3], des_ori[0], des_ori[1], des_ori[2]).matrix() des_angvel = quat_des.dot(des_angvel) quat_cur = pin.Quaternion(q[6], q[3], q[4], q[5]).matrix() cur_angvel = quat_cur.dot(cur_angvel) w_com = np.hstack([ m * np.multiply(self.kc, des_pos - com) + m * np.multiply(self.dc, des_vel - vcom), arr( arr(np.multiply(self.kb, quat_diff)) + (Ib * mat(np.multiply(self.db, des_angvel - cur_angvel))).T) ]) # adding weight # w_com[2] += m*9.81 return w_com
def integrate_dv_R3SO3(self, q, v, dv, dt): b_v = v[0:3] b_w = v[3:6] b_acc = dv[0:3] + np.cross(b_w, b_v) p_int = q[:3] oRb_int = pin.Quaternion(q[3:7].reshape((4, 1))).toRotationMatrix() v_int = oRb_int @ v[:3] p_int = p_int + v_int * dt + 0.5 * oRb_int @ b_acc * dt**2 v_int = v_int + oRb_int @ b_acc * dt oRb_int = oRb_int @ pin.exp(b_w * dt) q[:3] = p_int q[3:7] = pin.Quaternion(oRb_int).coeffs() q[7:] += v[6:] * dt v += dt * dv v[:3] = oRb_int.T @ v_int return q, v
def SphericalToRPY(joint): # i.e. SphericalToRPY('hip_r') i = pinocchioRobot.getDoFIdx(joint) quat = np.matrix([ pinocchioRobot.q[i, 0], pinocchioRobot.q[i + 1, 0], pinocchioRobot.q[i + 2, 0], pinocchioRobot.q[i + 3, 0] ], np.float) quat = np.squeeze(np.asarray(quat)) rpy = se3.utils.matrixToRpy( se3.Quaternion(quat[3], quat[0], quat[1], quat[2]).matrix()) return rpy
def objects_pos_orn_rand_falling(self): self.show_plane() dheight = 0.05 for n, body in enumerate(self.bodies): pos = self.np_random.uniform(*self.objects_xyz_interval) pos[2] = dheight * (n + 1) orn = pin.Quaternion(pin.SE3.Random().rotation).coeffs() body.pose = pos, orn ms = self.np_random.randint(5, 10) * 100 self.run_simulation(float(ms) * 1e-3)
def get_current_config(robot, graph, q0, timeout=5.): init_ros_node() import tf2_ros, rospy, pinocchio from geometry_msgs.msg import PoseWithCovarianceStamped from sensor_msgs.msg import JointState tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) # Build initial position msg = rospy.wait_for_message("/amcl_pose", PoseWithCovarianceStamped) #P = msg.pose.pose.position #Q = msg.pose.pose.orientation mMb = poseToSE3(msg.pose.pose) try: _wMm = tfBuffer.lookup_transform("world", msg.header.frame_id, msg.header.stamp, rospy.Duration(timeout)) wMm = poseToSE3(_wMm.transform) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: print('could not get TF transform : ', e) return wMb = wMm * mMb # conversion from # cos(t) = 2 cos(t/2)**2 - 1 # sin(t) = 2 cos(t/2) sin(t/2) P = wMb.translation Q = pinocchio.Quaternion(wMb.rotation) q = q0[:] q[0:4] = P[0], P[1], 2 * Q.w**2 - 1, 2 * Q.w * Q.z # Acquire robot state msg = rospy.wait_for_message("/joint_states", JointState) for ni, qi in zip(msg.name, msg.position): jni = "tiago/" + ni if robot.getJointConfigSize(jni) != 1: continue try: rk = robot.rankInConfiguration[jni] except KeyError: continue assert robot.getJointConfigSize(jni) == 1 q[rk] = qi # put driller in hand res, q0proj, err = graph.applyNodeConstraints( "tiago/gripper grasps driller/handle", q) if not res: print( "Could not project onto state 'tiago/gripper grasps driller/handle'", err) return q0proj
def dynamics(self, t, x): ''' Forward dynamics of the robot, to integrate ''' # Split input as (q, v) pair q = np.matrix(x[:self.robot.nq]).T v = np.matrix(x[self.robot.nq:]).T # Renormalize quaternion to prevent propagation of rounding errors due to integration. q[3:7, 0] /= np.linalg.norm(q[3:7, 0]) # Run forward dynamic computation # Compute H and g + coloriolis effects H = self.robot.mass(q) g = self.robot.nle(q, v) # Compute jacobian # Note that the jacobian only depends on data directly available in q, v, so there is no need for advanced # computation radius_world = se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).conjugate() * self.wheel_radius omega = v[3:6, 0] skew_radius = skew_symmetric(radius_world) omega_skew_radius = skew_symmetric(-skew_symmetric(omega) * radius_world) J = np.concatenate( (np.eye(3), skew_radius, skew_radius * np.matrix([[0, 1, 0]]).T), axis=1) dJ = np.concatenate((np.zeros((3, 3)), omega_skew_radius, omega_skew_radius * np.matrix([[0, 1, 0]]).T), axis=1) drift = -dJ * v # Compute controller torque torque = np.zeros((self.robot.model.nv, 1)) # Consider all joints actuated except freeflyer torque[6:, 0] = self.controller(t, q, v, None, None, None) # Write full equation A = np.block([[H, J.T], [J, np.zeros((3, 3))]]) # ~ b = np.concatenate((torque - g, drift)) b = np.concatenate((torque - g, drift)) # Find dv dv = np.linalg.solve(A, b)[:-3] # Now we just need to integrate dv # The slight issue is that q is not a term-by-term integral of v (because of the freeflyer). # Here we use pinocchio's integrate funciton to compute the numerical derivative of q dt = 1e-6 dq = ((se3.integrate(self.robot.model, q, dt * v) - q) / dt) return np.concatenate((dq, dv))
def read_data_file_laas(file_path, dt): # Load data file data = np.load(file_path) # o_a_oi: litteraly the imu ref frame origin acceleration (gravity is compensated!) arr_dic = { 'imu_acc': data['baseAccelerometer'], 'o_a_oi': data['baseLinearAcceleration'], 'o_q_i': data['baseOrientation'], 'i_omg_oi': data['baseAngularVelocity'], 'qa': data['q_mes'], 'dqa': data['v_mes'], 'tau': data['torquesFromCurrentMeasurment'], 'w_p_wm': data['mocapPosition'], 'w_q_m': data['mocapOrientationQuat'], 'w_R_m': data['mocapOrientationMat9'], 'w_v_wm': data['mocapVelocity'] } N = min(arr.shape[0] for key, arr in arr_dic.items()) arr_dic = {key: arr[:N, :] for key, arr in arr_dic.items()} # shortest time array fitting all traj t_arr = np.arange(N) * dt # in seconds, starting from 0 arr_dic['t'] = t_arr # !!!!! Mocap orientation needs to be inverted !!!!!! # quat -> conjugate # R mat -> transpose # arr_dic['w_q_m'][:,:3] = -arr_dic['w_q_m'][:,:3] # qx, qy, qz, qw -> -qx, -qy, -qz, qw # arr_dic['w_R_m'] = np.array([w_R_m.T for w_R_m in arr_dic['w_R_m']]) # other preprocessing (to unify formats) arr_dic['w_pose_wm'] = np.hstack([arr_dic['w_p_wm'], arr_dic['w_q_m']]) arr_dic['m_v_wm'] = np.array([ w_R_m.T @ w_v_wm for w_R_m, w_v_wm in zip(arr_dic['w_R_m'], arr_dic['w_v_wm']) ]) # compute local mocap base velocity arr_dic['o_R_i'] = np.array([ pin.Quaternion(o_q_i.reshape(4, 1)).toRotationMatrix() for o_q_i in arr_dic['o_q_i'] ]) # roll/pitch/yaw arr_dic['o_rpy_i'] = np.array( [pin.rpy.matrixToRpy(R) for R in arr_dic['o_R_i']]) arr_dic['w_rpy_m'] = np.array( [pin.rpy.matrixToRpy(R) for R in arr_dic['w_R_m']]) if 'contactStatus' in data: arr_dic['contactStatus'] = data['contactStatus'] return arr_dic
def test_matrixToRpy(self): n = 100 for _ in range(n): quat = pin.Quaternion(np.random.rand(4, 1)).normalized() R = quat.toRotationMatrix() v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2) self.assertTrue(-pi <= v[2] and v[2] <= pi) n2 = 100 # Test singular case theta = pi/2 Rp = np.array([[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]]) for _ in range(n2): r = random() * 2 * pi - pi y = random() * 2 * pi - pi R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \ .dot(Rp) \ .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix()) v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2) self.assertTrue(-pi <= v[2] and v[2] <= pi) # Test singular case theta = -pi/2 Rp = np.array([[0.0, 0.0, -1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]]) for _ in range(n2): r = random() * 2 * pi - pi y = random() * 2 * pi - pi R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \ .dot(Rp) \ .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix()) v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2) self.assertTrue(-pi <= v[2] and v[2] <= pi)
def compute_contact_forces(self, qa, dqa, o_R_b, b_v_ob, b_omg_ob, o_a_ob, tauj, world_frame=True): """ Compute the 3D contact/reaction forces at the solo feet using inverse dynamics. Forces are expressed in body frame. """ b_v_ob = np.zeros(3) o_quat_b = pin.Quaternion(o_R_b).coeffs() q = np.concatenate( [np.array([0, 0, 0]), o_quat_b, qa]) # robot anywhere with orientation estimated from IMU vq = np.concatenate( [b_v_ob, b_omg_ob, dqa]) # generalized velocity in base frame as pin requires # vq = np.hstack([np.zeros(3), b_omg_ob, dqa]) # generalized velocity in base frame as pin requires b_spa = o_R_b.T @ o_a_ob - np.cross( b_omg_ob, b_v_ob) # spatial acceleration linear part, neglecting the rest ddqa = ( dqa - self.dqa_prev ) / self.dt # using ddqa numdiff only seems to yield the best results # ddqa = np.zeros(12) # b_domgdt_ob = (b_omg_ob - self.b_omg_ob_prev)/self.dt b_domgdt_ob = np.zeros(3) dvq = np.concatenate([b_spa, b_domgdt_ob, ddqa]) taucf = pin.rnea( self.robot.model, self.robot.data, q, vq, dvq ) # sum of joint torque + contact torques corresponding to the actuated equations taucf[6:] = taucf[6:] - tauj self.dqa_prev = dqa self.b_omg_ob_prev = b_omg_ob return taucf_to_oforces(self.robot, q, self.nv, o_R_b, taucf, self.contact_frame_id_lst, world_frame=world_frame)
def UpdateMeasurment(self): """Retrieve data about the robot from the simulation to mimic what the masterboard does """ # Position and velocity of actuators jointStates = pyb.getJointStates(self.pyb_sim.robotId, self.revoluteJointIndices) # State of all joints self.q_mes[:] = np.array([state[0] for state in jointStates]) self.v_mes[:] = np.array([state[1] for state in jointStates]) # Measured torques self.torquesFromCurrentMeasurment[:] = self.jointTorques[:].ravel() # Position and orientation of the trunk (PyBullet world frame) self.baseState = pyb.getBasePositionAndOrientation(self.pyb_sim.robotId) self.dummyHeight = np.array(self.baseState[0]) self.dummyHeight[2] = 0.20 self.dummyPos = np.array(self.baseState[0]) # Linear and angular velocity of the trunk (PyBullet world frame) self.baseVel = pyb.getBaseVelocity(self.pyb_sim.robotId) # print("baseVel: ", np.array([self.baseVel[0]])) # Orientation of the base (quaternion) self.baseOrientation[:] = np.array(self.baseState[1]) RPY = quaternionToRPY(self.baseOrientation) self.hardware.roll = RPY[0] self.hardware.pitch = RPY[1] self.hardware.yaw = RPY[2] self.rot_oMb = pin.Quaternion(np.array([self.baseOrientation]).transpose()).toRotationMatrix() self.oMb = pin.SE3(self.rot_oMb, np.array([self.dummyHeight]).transpose()) # Angular velocities of the base self.baseAngularVelocity[:] = (self.oMb.rotation.transpose() @ np.array([self.baseVel[1]]).transpose()).ravel() # Linear Acceleration of the base self.o_baseVel = np.array([self.baseVel[0]]).transpose() self.b_baseVel = (self.oMb.rotation.transpose() @ self.o_baseVel).ravel() self.o_imuVel = self.o_baseVel + self.oMb.rotation @ self.cross3(np.array([0.1163, 0.0, 0.02]), self.baseAngularVelocity[:]) self.baseLinearAcceleration[:] = (self.oMb.rotation.transpose() @ (self.o_imuVel - self.prev_o_imuVel)).ravel() / self.dt self.prev_o_imuVel[:, 0:1] = self.o_imuVel self.baseAccelerometer[:] = self.baseLinearAcceleration[:] + \ (self.oMb.rotation.transpose() @ np.array([[0.0], [0.0], [-9.81]])).ravel() return
def velocityXYZQuatToXYZRPY(xyzquat: np.ndarray, v: np.ndarray) -> np.ndarray: """Convert the derivative of [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw]. .. note: No need to estimate yaw angle to get RPY velocity, since `computeRpyJacobianInverse` only depends on Roll and Pitch angles. However, it is not the case for the linear velocity. .. warning:: Linear velocity in XYZRPY must be local-world-aligned frame, while returned linear velocity in XYZQuat is in local frame. """ quat = pin.Quaternion(xyzquat[3:]) rpy = matrixToRpy(quat.matrix()) J_rpy_inv = computeRpyJacobianInverse(rpy) return np.concatenate((quat * v[:3], J_rpy_inv @ v[3:]))
def __init__(self, *args): if len(args) == 0: raise NotImplementedError elif len(args) == 7: raise NotImplementedError elif len(args) == 1: arg = args[0] if isinstance(arg, Transform): self.T = arg.T elif isinstance(arg, pin.SE3): self.T = arg else: arg_array = np.asarray(arg) if arg_array.shape == (4, 4): R = arg_array[:3, :3] t = arg_array[:3, -1] self.T = pin.SE3(R, t.reshape(3, 1)) else: raise NotImplementedError elif len(args) == 2: args_0_array = np.asarray(args[0]) n_elem_rot = len(args_0_array.flatten()) if n_elem_rot == 4: xyzw = np.asarray(args[0]).flatten().tolist() wxyz = [xyzw[-1], *xyzw[:-1]] assert len(wxyz) == 4 q = pin.Quaternion(*wxyz) q.normalize() R = q.matrix() elif n_elem_rot == 9: assert args_0_array.shape == (3, 3) R = args_0_array t = np.asarray(args[1]) assert len(t) == 3 self.T = pin.SE3(R, t.reshape(3, 1)) elif len(args) == 1: if isinstance(args[0], Transform): raise NotImplementedError elif isinstance(args[0], list): array = np.array(args[0]) assert array.shape == (4, 4) self.T = pin.SE3(array) elif isinstance(args[0], np.ndarray): assert args[0].shape == (4, 4) self.T = pin.SE3(args[0])
def compute_contact_forces2(self, qa, dqa, ddqa, o_R_i, i_omg_oi, i_domg_oi, o_a_oi, tauj, world_frame=True): """ Compute the 3D contact/reaction forces at the solo feet using inverse dynamics. Forces are expressed in body frame. """ zero3 = np.array([0, 0, 0]) o_quat_i = pin.Quaternion(o_R_i).coeffs() q = np.concatenate( [zero3, o_quat_i, qa]) # robot anywhere with orientation estimated from IMU vq = np.concatenate( [zero3, i_omg_oi, dqa]) # generalized velocity in base frame as pin requires i_a_oi = o_R_i.T @ o_a_oi # - np.cross(b_omg_ob, b_v_ob) # since the i_v_oi is set to zero arbitrarily dvq = np.concatenate([i_a_oi, i_domg_oi, ddqa]) taucf = pin.rnea( self.robot.model, self.robot.data, q, vq, dvq ) # sum of joint torque + contact torques corresponding to the actuated equations taucf[6:] = taucf[6:] - tauj self.dqa_prev = dqa self.i_omg_oi_prev = i_omg_oi return taucf_to_oforces(self.robot, q, self.nv, o_R_i, taucf, self.contact_frame_id_lst, world_frame=world_frame)
def check_pyb_env(self, qmes12): """Check the state of the robot to trigger events and update camera Args: qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators """ # Check if the robot is in front of the first sphere to trigger it if self.flag_sphere1 and (qmes12[1, 0] >= 0.9): pyb.resetBaseVelocity(self.sphereId1, linearVelocity=[3.0, 0.0, 2.0]) self.flag_sphere1 = False # Check if the robot is in front of the second sphere to trigger it if self.flag_sphere2 and (qmes12[1, 0] >= 1.1): pyb.resetBaseVelocity(self.sphereId2, linearVelocity=[-3.0, 0.0, 2.0]) self.flag_sphere2 = False # Apply perturbation # pyb.applyExternalForce(pyb_sim.robotId, -1, [1.0, 0.0, 0.0], [0.0, 0.0, 0.0], pyb.LINK_FRAME)""" # Update the PyBullet camera on the robot position to do as if it was attached to the robot """pyb.resetDebugVisualizerCamera(cameraDistance=0.75, cameraYaw=+50, cameraPitch=-35, cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0])""" oMb_tmp = pin.SE3(pin.Quaternion(qmes12[3:7]), np.array([0.0, 0.0, 0.0])) RPY = pin.rpy.matrixToRpy(oMb_tmp.rotation) # Update the PyBullet camera on the robot position to do as if it was attached to the robot pyb.resetDebugVisualizerCamera( cameraDistance=0.6, cameraYaw=(RPY[2, 0] * (180 / 3.1415) + 45), cameraPitch=-39.9, cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0]) return 0
def UpdateMeasurment(self): # Position and velocity of actuators jointStates = pyb.getJointStates( self.pyb_sim.robotId, self.revoluteJointIndices) # State of all joints self.q_mes[:] = np.array([state[0] for state in jointStates]) self.v_mes[:] = np.array([state[1] for state in jointStates]) # Measured torques self.torquesFromCurrentMeasurment[:] = self.jointTorques[:].ravel() # Position and orientation of the trunk (PyBullet world frame) self.baseState = pyb.getBasePositionAndOrientation( self.pyb_sim.robotId) self.dummyHeight = np.array(self.baseState[0]) self.dummyHeight[2] = 0.20 # Linear and angular velocity of the trunk (PyBullet world frame) self.baseVel = pyb.getBaseVelocity(self.pyb_sim.robotId) # Orientation of the base (quaternion) self.baseOrientation[:] = np.array(self.baseState[1]) self.rot_oMb = pin.Quaternion( np.array([self.baseState[1]]).transpose()).toRotationMatrix() self.oMb = pin.SE3(self.rot_oMb, np.array( [self.dummyHeight]).transpose()) # Angular velocities of the base self.baseAngularVelocity[:] = (self.oMb.rotation.transpose() @ np.array([ self.baseVel[1]]).transpose()).ravel() # Linear Acceleration of the base self.b_baseVel = (self.oMb.rotation.transpose() @ np.array( [self.baseVel[0]]).transpose()).ravel() self.baseLinearAcceleration[:] = ( self.b_baseVel.ravel() - self.prev_b_baseVel) / self.dt self.prev_b_baseVel[:] = self.b_baseVel.ravel() return
def extract_new_positions(self): # This function needs to return: # 1: end_effector position # 2: end_effector velocity # 3: end_effector orientation current_c_pos = np.array( self.data.oMf[self.end_effector_frame_id].translation) current_c_pos += self.offset # should return 0 always as we do not give torques # current_c_vel = np.array(pinocchio.getFrameVelocity(self.model, self.data, # self.model.getFrameId('panda_grasptarget'))) current_c_vel = np.zeros(3) # current_c_vel = (current_c_pos - self.c_pos_old)*self.dt rotation_matrix = np.array( self.data.oMf[self.end_effector_frame_id].rotation) quat_pin = pinocchio.Quaternion(self.data.oMf[ self.end_effector_frame_id].rotation).coeffs() # [ x, y, z, w] current_c_quat = np.zeros(4) current_c_quat[1:] = quat_pin[:3] current_c_quat[0] = quat_pin[-1] return current_c_pos, current_c_vel, current_c_quat
for i in range(N): M = se3.exp(nu) * M viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M)) time.sleep(1) # Integrate a velocity of the body that is constant in the world frame. for i in range(N): Mc = se3.SE3(M.rotation, zero(3)) M = M * se3.exp(Mc.actInv(nu)) viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M)) time.sleep(1) # Integrate a constant "log" velocity in body frame. ME = se3.SE3( se3.Quaternion(0.7, -0.6, 0.1, 0.4).normalized().matrix(), np.matrix([1, -1, 2], np.double).T) nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N) for i in range(N): M = M * se3.exp(nu) viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M)) print "Residuals = ", norm(se3.log(M.inverse() * ME).vector()) time.sleep(1) # Integrate a constant "log" velocity in reference frame. ME = se3.SE3( se3.Quaternion(0.3, -0.2, 0.6, 0.5).normalized().matrix(), np.matrix([-1, 1, 0.6], np.double).T) nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N) for i in range(N): M = M * se3.exp(nu)
ax[j].set_ylim([ np.min(x[j, :]) - 0.1 * (X_MAX[j] - X_MIN[j]), np.max(x[j, :]) + 0.1 * (X_MAX[j] - X_MIN[j]) ]) ax[6].set_xlabel(xlabel) ax[7].set_xlabel(xlabel) ax[0].set_title(name) #plut.saveFigure(TEST_NAME+'_'+name); plut.saveFigureandParameterinDateFolder(GARBAGE_FOLDER, TEST_NAME + '_' + name, PARAMS) return ax # Convertion from translation+quaternion to SE3 and reciprocally q2m = lambda q: se3.SE3( se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).array(), q[:3] ) # matrix m2q = lambda M: np.vstack([M.translation, se3.Quaternion(M.rotation).coeffs()]) ''' PLOT-RELATED USER PARAMETERS ''' LW = 4 # line width LINE_ALPHA = 1.0 LEGEND_ALPHA = 1.0 line_styles = ["c-", "b--", "g-.", "k:", "m-"] PLOT_JOINT_POS_VEL_ACC = True PLOT_STATE_SPACE = 1 Q_INTERVAL = 0.001 # the range of possible angles is sampled with this step for plotting TEST_STANDARD = 1 TEST_VIABILITY = 0 TEST_RANDOM = 0
def XYZQuatToXYZRPY(xyzquat): """Convert [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw]. """ return np.concatenate(( xyzquat[:3], matrixToRpy(pin.Quaternion(xyzquat[3:]).matrix())))
if RUN_SIMULATION: t1 = time.time() try: subprocess.run(RUN_FILE, stdout=subprocess.DEVNULL, check=True) except subprocess.CalledProcessError as e: raise (e) print('time: ', time.time() - t1) df_gtr = pd.read_csv('gtr.csv') df_est = pd.read_csv('est.csv') df_fbk = pd.read_csv('fbk.csv') df_kfs = pd.read_csv('kfs.csv') # compute angle axis quat_gtr_lst = [ pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip( df_gtr['qx'], df_gtr['qy'], df_gtr['qz'], df_gtr['qw']) ] quat_est_lst = [ pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip( df_est['qx'], df_est['qy'], df_est['qz'], df_est['qw']) ] quat_fbk_lst = [ pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip( df_fbk['qx'], df_fbk['qy'], df_fbk['qz'], df_fbk['qw']) ] quat_kfs_lst = [ pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip( df_kfs['qx'], df_kfs['qy'], df_kfs['qz'], df_kfs['qw']) ]
import os import sys import crocoddyl import pinocchio import numpy as np import example_robot_data WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ hector = example_robot_data.load('hector') robot_model = hector.model target_pos = np.array([1., 0., 1.]) target_quat = pinocchio.Quaternion(1., 0., 0., 0.) state = crocoddyl.StateMultibody(robot_model) d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5., 0.1 tau_f = np.array([[0., 0., 0., 0.], [0., 0., 0., 0.], [1., 1., 1., 1.], [0., d_cog, 0., -d_cog], [-d_cog, 0., d_cog, 0.], [-cm / cf, cm / cf, -cm / cf, cm / cf]]) actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f) nu = actuation.nu runningCostModel = crocoddyl.CostModelSum(state, nu) terminalCostModel = crocoddyl.CostModelSum(state, nu) # Costs xResidual = crocoddyl.ResidualModelState(state, state.zero(), nu) xActivation = crocoddyl.ActivationModelWeightedQuad(np.array([0.1] * 3 + [1000.] * 3 + [1000.] * robot_model.nv))
import os import sys import crocoddyl import pinocchio import numpy as np import example_robot_data WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ WITHDISPLAY = True hector = example_robot_data.loadHector() robot_model = hector.model target_pos = np.array([1, 0, 1]) target_quat = pinocchio.Quaternion(1, 0, 0, 0) state = crocoddyl.StateMultibody(robot_model) d_cog = 0.1525 cf = 6.6e-5 cm = 1e-6 u_lim = 5 l_lim = 0.1 tau_f = np.array([[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [1.0, 1.0, 1.0, 1.0], [0.0, d_cog, 0.0, -d_cog], [-d_cog, 0.0, d_cog, 0.0], [-cm / cf, cm / cf, -cm / cf, cm / cf]]) actModel = crocoddyl.ActuationModelMultiCopterBase(state, 4, tau_f) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
def logarithmicMap(self, quat_wxyz): w, x, y, z = quat_wxyz return pin.log(pin.Quaternion(w, x, y, z).matrix())