def test_frame_algo(self): model = self.model data = model.createData() q = pin.neutral(model) v = np.random.rand((model.nv)) frame_id = self.frame_idx J1 = pin.computeFrameJacobian(model, data, q, frame_id) J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL) self.assertApprox(J1, J2) data2 = model.createData() pin.computeJointJacobians(model, data2, q) J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL) self.assertApprox(J1, J3) dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id, pin.LOCAL) data3 = model.createData() pin.computeJointJacobiansTimeVariation(model, data3, q, v) dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id, pin.LOCAL) self.assertApprox(dJ1, dJ2)
def invgeom(h = 0.2, lx = 0.1946, ly=0.16891, leg_dir = [+1,+1,-1,-1]): #Inputs to be modified by the user feet_position_ref = [np.array([lx, ly, 0.0191028]),np.array([lx, -ly, 0.0191028]),np.array([-lx, ly, 0.0191028]),np.array([-lx, -ly, 0.0191028])] base_orientation_ref = pin.utils.rpyToMatrix(0,0,0) com_ref = np.array([0,0,h]) FL_FOOT_ID = robot.model.getFrameId('FL_FOOT') FR_FOOT_ID = robot.model.getFrameId('FR_FOOT') HL_FOOT_ID = robot.model.getFrameId('HL_FOOT') HR_FOOT_ID = robot.model.getFrameId('HR_FOOT') BASE_ID = robot.model.getFrameId('base_link') foot_ids = np.array([FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID]) q = np.array([ 0., 0.,0.235, 0. , 0. , 0. , 1. , 0.1 , +0.8 * leg_dir[0] , -1.6 * leg_dir[0] , -0.1 , +0.8 * leg_dir[1], -1.6 * leg_dir[1] , 0.1 , -0.8 * leg_dir[2] , +1.6 * leg_dir[2] , -0.1 , -0.8 * leg_dir[3] , +1.6 * leg_dir[3]]) dq = robot.v0.copy() for i in range(100): #Compute fwd kin pin.forwardKinematics(rmodel,rdata,q,np.zeros(rmodel.nv),np.zeros(rmodel.nv)) pin.updateFramePlacements(rmodel,rdata) ### FEET Jfeet = [] pfeet_err = [] for i_ee in range(4): idx = int(foot_ids[i_ee]) pos = rdata.oMf[idx].translation ref = feet_position_ref[i_ee] Jfeet.append(pin.computeFrameJacobian(robot.model,robot.data,q,idx,pin.LOCAL_WORLD_ALIGNED)[:3]) pfeet_err.append(ref-pos) ### CoM com = robot.com(q) Jcom = robot.Jcom(q) e_com = com_ref-com ### BASE ROTATION idx = BASE_ID rot = rdata.oMf[idx].rotation rotref = base_orientation_ref Jwbasis = pin.computeFrameJacobian(robot.model,robot.data,q,idx,pin.LOCAL_WORLD_ALIGNED)[3:] e_basisrot = -rotref @ pin.log3(rotref.T@rot) #All tasks J = np.vstack(Jfeet+[Jcom,Jwbasis]) x_err = np.concatenate(pfeet_err+[e_com,e_basisrot]) residual = np.linalg.norm(x_err) if residual<1e-5: print (np.linalg.norm(x_err)) print ("iteration: {} residual: {}".format(i,residual)) return q dq=np.linalg.pinv(J)@(x_err) q=pin.integrate(robot.model,q,dq) return q * np.nan
def compute_jacobian(self, q): ''' computes the jacobian in the world frame Math : J = R(World,Foot) * J_(Foot frame) Selection of the required portion of the jacobian is also done here ''' self.compute_forward_kinematics(q) jac = pin.computeFrameJacobian(self.pin_robot.model, self.pin_robot.data, q, self.frame_end_idx) jac = self.pin_robot.data.oMf[self.frame_end_idx].rotation.dot( jac[0:3]) return jac
def compute_relative_velocity_between_frames(self, q, dq): ''' computes the velocity of the end_frame with respect to a frame whose origin aligns with the root frame but is oriented as the world frame ''' # TODO: define relative vel with respect to frame oriented as the base frame but located at root frame ## will be a problem in case of a back flip with current implementation. frame_config_root = pin.SE3( self.pin_robot.data.oMf[self.frame_root_idx].rotation, np.zeros((3, 1))) frame_config_end = pin.SE3( self.pin_robot.data.oMf[self.frame_end_idx].rotation, np.zeros((3, 1))) vel_root_in_world_frame = frame_config_root.action.dot( pin.computeFrameJacobian(self.pin_robot.model, self.pin_robot.data, q, self.frame_root_idx)).dot(dq)[0:3] vel_end_in_world_frame = frame_config_end.action.dot( pin.computeFrameJacobian(self.pin_robot.model, self.pin_robot.data, q, self.frame_end_idx)).dot(dq)[0:3] return np.subtract(vel_end_in_world_frame, vel_root_in_world_frame).T
def compute_jacobian(self, finger_id, q0): """ Compute the jacobian of a finger at configuration q0. Args: finger_id (int): an index specifying the end effector. (i.e. 0 for the first finger, 1 for the second, ...) q0 (np.array): The current joint positions. Returns: An np.array containing the jacobian. """ frame_id = self.tip_link_ids[finger_id] return pinocchio.computeFrameJacobian( self.robot_model, self.data, q0, frame_id, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED, )
def id_joint_torques(self, q, dq, des_q, des_v, des_a, fff, cnt_array): """ This function computes the input torques with gains Input: q : joint positions dq : joint velocity des_q : desired joint positions des_v : desired joint velocities des_a : desired joint accelerations fff : desired feed forward force cnt_array """ assert len(q) == self.nq self.J_arr = [] tau_id = self.compute_id_torques(des_q, des_v, des_a) ## creating QP matrices N = int(len(self.eff_arr)) for j in range(N): self.J_arr.append(pin.computeFrameJacobian(self.pinModel, self.pinData, des_q,\ self.pinModel.getFrameId(self.eff_arr[j]), pin.LOCAL_WORLD_ALIGNED).T) tau_eff = np.zeros(self.nv) for j in range(N): if fff[(j * 3) + 2] > 0: tau_eff += np.matmul( self.J_arr[j], np.hstack((fff[j * 3:(j + 1) * 3], np.zeros(3)))) tau = (tau_id - tau_eff)[6:] tau_gain = -self.kp * (np.subtract(q[7:].T, des_q[7:].T)) - self.kd * ( np.subtract(dq[6:].T, des_v[6:].T)) return tau + tau_gain.T
def calcDiff(self, q): J = pin.computeFrameJacobian(self.rmodel, self.rdata, q, self.frameIndex) r = self.residual(q) Jlog = pin.Jlog6(self.deltaM) return 2 * J.T @ Jlog.T @ r
def kinInv_3D(self, q, qdot, solo, feet_traj): from numpy.linalg import pinv K = self.getParameter('kinv_gain') # Convergence gain # unactuated, [x, y, z] position of the base + [x, y, z, w] orientation of the base (stored as a quaternion) # qu = q[:7] # [v_x, v_y, v_z] linear velocity of the base and [w_x, w_y, w_z] angular velocity of the base along x, y, z axes # of the world # qu_dot = qdot[:6] qa_dot_ref = np.zeros( (12, 1)) # target angular velocities for the motors if self.flag_q_ref: self.q_ref = solo.q0.copy() self.flag_q_ref = False # Compute/update all the joints and frames pin.forwardKinematics(solo.model, solo.data, self.q_ref) pin.updateFramePlacements(solo.model, solo.data) # Get 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") # Get the current position of the feet xyz_FL = solo.data.oMf[ID_FL].translation xyz_FR = solo.data.oMf[ID_FR].translation xyz_HL = solo.data.oMf[ID_HL].translation xyz_HR = solo.data.oMf[ID_HR].translation # Desired foot trajectory xyzdes_FL = feet_traj[0] xyzdes_FR = feet_traj[1] xyzdes_HL = feet_traj[2] xyzdes_HR = feet_traj[3] # Calculating the error err_FL = (xyz_FL - xyzdes_FL) err_FR = (xyz_FR - xyzdes_FR) err_HL = (xyz_HL - xyzdes_HL) err_HR = (xyz_HR - xyzdes_HR) # Computing the local Jacobian into the global frame oR_FL = solo.data.oMf[ID_FL].rotation oR_FR = solo.data.oMf[ID_FR].rotation oR_HL = solo.data.oMf[ID_HL].rotation oR_HR = solo.data.oMf[ID_HR].rotation # Getting the different Jacobians fJ_FL3 = pin.computeFrameJacobian( solo.model, solo.data, self.q_ref, ID_FL)[:3, -12:] # Take only the translation terms oJ_FL3 = oR_FL.dot( fJ_FL3) # Transformation from local frame to world frame oJ_FLxyz = oJ_FL3[0:3, -12:] # Take the x,y & z components fJ_FR3 = pin.computeFrameJacobian(solo.model, solo.data, self.q_ref, ID_FR)[:3, -12:] oJ_FR3 = oR_FR.dot(fJ_FR3) oJ_FRxyz = oJ_FR3[0:3, -12:] fJ_HL3 = pin.computeFrameJacobian(solo.model, solo.data, self.q_ref, ID_HL)[:3, -12:] oJ_HL3 = oR_HL.dot(fJ_HL3) oJ_HLxyz = oJ_HL3[0:3, -12:] fJ_HR3 = pin.computeFrameJacobian(solo.model, solo.data, self.q_ref, ID_HR)[:3, -12:] oJ_HR3 = oR_HR.dot(fJ_HR3) oJ_HRxyz = oJ_HR3[0:3, -12:] # Displacement error nu = np.hstack([err_FL, err_FR, err_HL, err_HR]) nu = np.matrix(nu) nu = np.transpose(nu) # Making a single x&y&z-rows Jacobian vector J = np.vstack([oJ_FLxyz, oJ_FRxyz, oJ_HLxyz, oJ_HRxyz]) # Computing the velocity qa_dot_ref = -K * pinv(J) * nu q_dot_ref = np.concatenate((np.zeros([6, 1]), qa_dot_ref)) # Computing the updated configuration self.q_ref = pin.integrate(solo.model, self.q_ref, q_dot_ref * self.getParameter('dt')) qa_ref = self.q_ref[7:].reshape(12, 1) # Return configuration of the robot q_dot_ref = np.squeeze(np.array(q_dot_ref)) err = np.linalg.norm(nu) return self.q_ref, q_dot_ref, err
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) q = q0.copy() herr = [] for i in range(1000): # Integrate over 2 second of robot life pio.framesForwardKinematics(robot.model, robot.data,
def compute(self, q, dq): ### FEET Jfeet = [] afeet = [] pfeet_err = [] vfeet_ref = [] pin.forwardKinematics(self.rmodel, self.rdata, q, dq, np.zeros(self.rmodel.nv)) pin.updateFramePlacements(self.rmodel, self.rdata) for i_ee in range(4): idx = int(self.foot_ids[i_ee]) pos = self.rdata.oMf[idx].translation nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED) ref = self.feet_position_ref[i_ee] vref = self.feet_velocity_ref[i_ee] aref = self.feet_acceleration_ref[i_ee] J1 = pin.computeFrameJacobian(self.robot.model, self.robot.data, q, idx, pin.LOCAL_WORLD_ALIGNED)[:3] e1 = ref - pos acc1 = -self.Kp_flyingfeet * (pos - ref) - self.Kd_flyingfeet * ( nu.linear - vref) + aref if self.flag_in_contact[i_ee]: acc1 *= 0 # In contact = no feedback drift1 = np.zeros(3) drift1 += pin.getFrameAcceleration(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED).linear drift1 += self.cross3(nu.angular, nu.linear) acc1 -= drift1 Jfeet.append(J1) afeet.append(acc1) pfeet_err.append(e1) vfeet_ref.append(vref) ### BASE POSITION idx = self.BASE_ID pos = self.rdata.oMf[idx].translation nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED) ref = self.base_position_ref Jbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data, q, idx, pin.LOCAL_WORLD_ALIGNED)[:3] e_basispos = ref - pos accbasis = -self.Kp_base_position * ( pos - ref) - self.Kd_base_position * nu.linear drift = np.zeros(3) drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED).linear drift += self.cross3(nu.angular, nu.linear) accbasis -= drift ### BASE ROTATION idx = self.BASE_ID rot = self.rdata.oMf[idx].rotation nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED) rotref = self.base_orientation_ref Jwbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data, q, idx, pin.LOCAL_WORLD_ALIGNED)[3:] e_basisrot = -rotref @ pin.log3(rotref.T @ rot) accwbasis = -self.Kp_base_orientation * rotref @ pin.log3( rotref.T @ rot) - self.Kd_base_orientation * nu.angular drift = np.zeros(3) drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED).angular accwbasis -= drift J = np.vstack(Jfeet + [Jbasis, Jwbasis]) acc = np.concatenate(afeet + [accbasis, accwbasis]) x_err = np.concatenate(pfeet_err + [e_basispos, e_basisrot]) dx_ref = np.concatenate( vfeet_ref + [self.base_linearvelocity_ref, self.base_angularvelocity_ref]) invJ = self.dinv(J) #or np.linalg.inv(J) since full rank ddq = invJ @ acc self.q_cmd = pin.integrate(self.robot.model, q, invJ @ x_err) self.dq_cmd = invJ @ dx_ref return ddq
def calcDiff6d(self, q): J = pin.computeFrameJacobian(robot.model, robot.data, q, self.frameIndex) r = self.residual6d(q) Jlog = pin.Jlog6(self.deltaM) return 2 * J.T @ Jlog.T @ r
def c_walking_IK(q, qdot, dt, solo, t_simu): qu = q[: 7] # unactuated, [x, y, z] position of the base + [x, y, z, w] orientation of the base (stored as a quaternion) qa = q[7:] # actuated, [q1, q2, ..., q8] angular position of the 8 motors qu_dot = qdot[: 6] # [v_x, v_y, v_z] linear velocity of the base and [w_x, w_y, w_z] angular velocity of the base along x, y, z axes of the world qa_dot = qdot[6:] # angular velocity of the 8 motors qa_ref = np.zeros((8, 1)) # target angular positions for the motors qa_dot_ref = np.zeros((8, 1)) # target angular velocities for the motors ############################################### # Insert code here to set qa_ref and qa_dot_ref from numpy.linalg import pinv global q_ref, flag_q_ref, T, dx, dz if flag_q_ref: q_ref = solo.q0.copy() flag_q_ref = False # Initialization of the variables K = 100. #Convergence gain T = 0.3 #period of the foot trajectory xF0 = 0.19 #initial position of the front feet xH0 = -0.19 #initial position of the hind feet z0 = 0.0 #initial altitude of each foot dx = 0.05 #displacement amplitude by x dz = 0.1 #displacement amplitude by z # Get 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") # function defining the feet's trajectory def ftraj(t, x0, z0): #arguments : time, initial position x and z global T, dx, dz x = [] z = [] if t >= T: t %= T x.append(x0 - dx * np.cos(2 * np.pi * t / T)) if t <= T / 2.: z.append(z0 + dz * np.sin(2 * np.pi * t / T)) else: z.append(0) return np.matrix([x, z]) # Compute/update all the joints and frames pin.forwardKinematics(solo.model, solo.data, q_ref) pin.updateFramePlacements(solo.model, solo.data) # Get the current height (on axis z) and the x-coordinate of the front left foot xz_FL = solo.data.oMf[ID_FL].translation[0::2] xz_FR = solo.data.oMf[ID_FR].translation[0::2] xz_HL = solo.data.oMf[ID_HL].translation[0::2] xz_HR = solo.data.oMf[ID_HR].translation[0::2] # Desired foot trajectory xzdes_FL = ftraj(t_simu, xF0, z0) xzdes_HR = ftraj(t_simu, xH0, z0) xzdes_FR = ftraj(t_simu + T / 2, xF0, z0) xzdes_HL = ftraj(t_simu + T / 2, xH0, z0) # Calculating the error err_FL = xz_FL - xzdes_FL err_FR = xz_FR - xzdes_FR err_HL = xz_HL - xzdes_HL err_HR = xz_HR - xzdes_HR # Computing the local Jacobian into the global frame oR_FL = solo.data.oMf[ID_FL].rotation oR_FR = solo.data.oMf[ID_FR].rotation oR_HL = solo.data.oMf[ID_HL].rotation oR_HR = solo.data.oMf[ID_HR].rotation # Getting the different Jacobians fJ_FL3 = pin.computeFrameJacobian( solo.model, solo.data, q_ref, ID_FL)[:3, -8:] #Take only the translation terms oJ_FL3 = oR_FL * fJ_FL3 #Transformation from local frame to world frame oJ_FLxz = oJ_FL3[0::2, -8:] #Take the x and z components fJ_FR3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_FR)[:3, -8:] oJ_FR3 = oR_FR * fJ_FR3 oJ_FRxz = oJ_FR3[0::2, -8:] fJ_HL3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_HL)[:3, -8:] oJ_HL3 = oR_HL * fJ_HL3 oJ_HLxz = oJ_HL3[0::2, -8:] fJ_HR3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_HR)[:3, -8:] oJ_HR3 = oR_HR * fJ_HR3 oJ_HRxz = oJ_HR3[0::2, -8:] # Displacement error nu = np.vstack([err_FL, err_FR, err_HL, err_HR]) # Making a single x&z-rows Jacobian vector J = np.vstack([oJ_FLxz, oJ_FRxz, oJ_HLxz, oJ_HRxz]) # Computing the velocity qa_dot_ref = -K * pinv(J) * nu q_dot_ref = np.concatenate((np.zeros([6, 1]), qa_dot_ref)) # Computing the updated configuration q_ref = pin.integrate(solo.model, q_ref, q_dot_ref * dt) qa_ref = q_ref[7:] # DONT FORGET TO RUN GEPETTO-GUI BEFORE RUNNING THIS PROGRAMM # #solo.display(q) # display the robot in the viewer Gepetto-GUI given its configuration q q_list.append(qa_ref) # End of the control code ############################################### # Parameters for the PD controller Kp = 10. Kd = 0.3 torque_sat = 3 # torque saturation in N.m torques_ref = np.zeros((8, 1)) # feedforward torques # Call the PD controller torques = PD(qa_ref, qa_dot_ref, qa, qa_dot, dt, Kp, Kd, torque_sat, torques_ref) # torques must be a numpy array of shape (8, 1) containing the torques applied to the 8 motors return torques, q_list
def c_walking_IK_bezier(q, qdot, dt, solo, t_simu): qu = q[: 7] # unactuated, [x, y, z] position of the base + [x, y, z, w] orientation of the base (stored as a quaternion) qa = q[7:] # actuated, [q1, q2, ..., q8] angular position of the 8 motors qu_dot = qdot[: 6] # [v_x, v_y, v_z] linear velocity of the base and [w_x, w_y, w_z] angular velocity of the base along x, y, z axes of the world qa_dot = qdot[6:] # angular velocity of the 8 motors qa_ref = np.zeros((8, 1)) # target angular positions for the motors qa_dot_ref = np.zeros((8, 1)) # target angular velocities for the motors ############################################### # Insert code here to set qa_ref and qa_dot_ref global q_ref, flag_q_ref, T if flag_q_ref: q_ref = solo.q0.copy() flag_q_ref = False # Get 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") # Compute/update all the joints and frames pin.forwardKinematics(solo.model, solo.data, q_ref) pin.updateFramePlacements(solo.model, solo.data) # Get the current height (on axis z) and the x-coordinate of the front left foot xz_FL = solo.data.oMf[ID_FL].translation[0::2] xz_FR = solo.data.oMf[ID_FR].translation[0::2] xz_HL = solo.data.oMf[ID_HL].translation[0::2] xz_HR = solo.data.oMf[ID_HR].translation[0::2] # Desired foot trajectory xzdes_FL = ftraj_Front(t_simu) xzdes_HR = ftraj_Hind(t_simu) xzdes_FR = ftraj_Front(t_simu + T / 2) xzdes_HL = ftraj_Hind(t_simu + T / 2) # Calculating the error err_FL = xz_FL - xzdes_FL err_FR = xz_FR - xzdes_FR err_HL = xz_HL - xzdes_HL err_HR = xz_HR - xzdes_HR # Computing the local Jacobian into the global frame oR_FL = solo.data.oMf[ID_FL].rotation oR_FR = solo.data.oMf[ID_FR].rotation oR_HL = solo.data.oMf[ID_HL].rotation oR_HR = solo.data.oMf[ID_HR].rotation # Getting the different Jacobians fJ_FL3 = pin.computeFrameJacobian( solo.model, solo.data, q_ref, ID_FL)[:3, -8:] #Take only the translation terms oJ_FL3 = oR_FL * fJ_FL3 #Transformation from local frame to world frame oJ_FLxz = oJ_FL3[0::2, -8:] #Take the x and z components fJ_FR3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_FR)[:3, -8:] oJ_FR3 = oR_FR * fJ_FR3 oJ_FRxz = oJ_FR3[0::2, -8:] fJ_HL3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_HL)[:3, -8:] oJ_HL3 = oR_HL * fJ_HL3 oJ_HLxz = oJ_HL3[0::2, -8:] fJ_HR3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_HR)[:3, -8:] oJ_HR3 = oR_HR * fJ_HR3 oJ_HRxz = oJ_HR3[0::2, -8:] # Displacement error nu = np.vstack([err_FL, err_FR, err_HL, err_HR]) # Making a single x&z-rows Jacobian vector J = np.vstack([oJ_FLxz, oJ_FRxz, oJ_HLxz, oJ_HRxz]) # Computing the velocity qa_dot_ref = -K * pinv(J) * nu q_dot_ref = np.concatenate((np.zeros([6, 1]), qa_dot_ref)) # Computing the updated configuration q_ref = pin.integrate(solo.model, q_ref, q_dot_ref * dt) qa_ref = q_ref[7:] # DONT FORGET TO RUN GEPETTO-GUI BEFORE RUNNING THIS PROGRAMM # solo.display( q ) # display the robot in the viewer Gepetto-GUI given its configuration q # End of the control code ############################################### # Parameters for the PD controller Kp = 10. Kd = 0.3 torque_sat = 2.5 # torque saturation in N.m torques_ref = np.zeros((8, 1)) # feedforward torques # Call the PD controller torques = PD(qa_ref, qa_dot_ref, qa, qa_dot, dt, Kp, Kd, torque_sat, torques_ref) # torques must be a numpy array of shape (8, 1) containing the torques applied to the 8 motors return torques, qa_ref, qa_dot_ref
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 gaze and ball. # 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 oMgaze = robot.data.oMf[ IDX_GAZE] # Placement from world frame o to frame f oMgaze oRgaze = oMgaze.rotation # Rotation from world axes to gaze axes oRgaze gaze_Jgaze = pio.computeFrameJacobian( robot.model, robot.data, q, IDX_GAZE) # 6D jacobian in local frame o_Jgaze3 = oRgaze * gaze_Jgaze[:3, :] # 3D jacobian in world frame o_GazeBall = oMgaze.translation - ball # vector from gaze to ball, in world frame vq = -pinv(o_Jgaze3) * o_GazeBall q = pio.integrate(robot.model, q, vq * DT) robot.display(q) time.sleep(1e-3) herr.append(o_GazeBall) q = q0.copy() herr = [] # Log the value of the error between tool and goal. herr2 = [] # Log the value of the error between gaze and ball. # Loop on an inverse kinematics for 200 iterations.