def _getCollisionJacobian(self, col, res): '''Compute the jacobian for one collision only. ''' contact = res.getContact(0) g1 = self.gmodel.geometryObjects[col.first] g2 = self.gmodel.geometryObjects[col.second] oMc = pio.SE3( pio.Quaternion.FromTwoVectors( np.matrix([0, 0, 1]).T, contact.normal).matrix(), contact.pos) joint1 = g1.parentJoint joint2 = g2.parentJoint oMj1 = self.rdata.oMi[joint1] oMj2 = self.rdata.oMi[joint2] cMj1 = oMc.inverse() * oMj1 cMj2 = oMc.inverse() * oMj2 J1 = pio.getJointJacobian(self.rmodel, self.rdata, joint1, pio.ReferenceFrame.LOCAL) J2 = pio.getJointJacobian(self.rmodel, self.rdata, joint2, pio.ReferenceFrame.LOCAL) Jc1 = cMj1.action * J1 Jc2 = cMj2.action * J2 J = (Jc1 - Jc2)[2, :] return J
def Kid(q_, J_=None): pinocchio.computeJointJacobians(model, data, q_) J = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy() if J_ is not None: J_[:, :] = J M = pinocchio.crba(model, data, q_).copy() return np.bmat([[M, J.T], [J, zero([6, 6])]])
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) # THIS FUNCTION CALLS THE FORWARD KINEMATICS computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.WORLD) # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) # Print out the placement of each joint of the kinematic tree frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.WORLD)
def comp_combined_force(self, q, des_force, des_torque, in_touch): if (np.sum(in_touch) == 0): return np.zeros((9)), np.zeros((9)) q = self.trafo_q(q) # TODO: I think it is sufficient if this is calculated only once: full_JAC = pin.computeJointJacobians(self.model, self.data, q) J_trans_inv_mat = np.zeros((3, 3 * 12)) for i in range(3): if (in_touch[i] == 1): idx = i * 4 + 4 idx_low = idx - 4 J_NEW = pin.getJointJacobian( self.model, self.data, idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :] J_trans = J_NEW.T J_trans_inv = np.matmul( np.linalg.pinv(np.matmul(J_trans.T, J_trans)), J_trans.T) J_trans_inv_mat[:, idx_low * 3:idx * 3] = J_trans_inv add_torque_force = np.matmul( np.matmul( np.linalg.pinv(np.matmul(J_trans_inv_mat.T, J_trans_inv_mat)), J_trans_inv_mat.T), des_force) add_torque_force1 = self.inv_trafo_q(add_torque_force[:12]) add_torque_force2 = self.inv_trafo_q(add_torque_force[12:24]) add_torque_force3 = self.inv_trafo_q(add_torque_force[24:36]) return (add_torque_force1 + add_torque_force2 + add_torque_force3), np.zeros((9))
def dresidualWorld(q): pinocchio.forwardKinematics(rmodel, rdata, q) pinocchio.computeJointJacobians(rmodel, rdata, q) rMi = Mref.inverse() * rdata.oMi[jid] return np.dot( pinocchio.Jlog6(rMi), pinocchio.getJointJacobian(rmodel, rdata, jid, pinocchio.ReferenceFrame.WORLD))
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) # Perform the forward kinematics over the kinematic tree forwardKinematics(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel) computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_velocity = getVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_linear_vel = joint_velocity.linear joint_angular_vel = joint_velocity.angular joint_6v_vel = joint_velocity.vector err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_velocity = getFrameVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_linear_vel = frame_velocity.linear frame_angular_vel = frame_velocity.angular frame_6v_vel = frame_velocity.vector err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def cid(q_, v_, tau_): pinocchio.computeJointJacobians(model, data, q_) J6 = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy() J = J6[:3, :] b = pinocchio.rnea(model, data, q_, v_, zero(model.nv)).copy() M = pinocchio.crba(model, data, q_).copy() pinocchio.forwardKinematics(model, data, q_, v_, zero(model.nv)) gamma = data.a[-1].linear + cross(data.v[-1].angular, data.v[-1].linear) K = np.bmat([[M, J.T], [J, zero([3, 3])]]) r = np.concatenate([tau_ - b, -gamma]) return inv(K) * r
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) numpy_vector_joint_acc = np.random.rand(model.njoints - 1) # Calls Reverese Newton-Euler algorithm numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel, numpy_vector_joint_acc) # IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_velocity = getVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) err = joint_velocity.vector - \ joint_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_velocity = getFrameVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) err = frame_velocity.vector - \ frame_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def imp_ctrl(self, q, force_dir, joint_id, vel, pos, v, K_xy, K_z): q = self.trafo_q(q) v = self.trafo_q(v) m = 1.0 d_xy = np.sqrt(4 * m * 500) d_z = np.sqrt(4 * m * 500) M = np.eye(3) * m M_inv = np.linalg.pinv(M) D = np.eye(3) D[0, 0] = D[1, 1] = d_xy D[2, 2] = d_z K = np.eye(3) K[0, 0] = K_xy K[1, 1] = K_xy K[2, 2] = K_z F = 0.01 #0.05 M_joint = pin.crba(self.model, self.data, q) pos_gain = np.matmul(K, pos) damp_gain = np.matmul(D, vel) # damp_gain = 0.0 force_gain = F * force_dir acc = np.matmul(M_inv, (force_gain - pos_gain - damp_gain)) # TODO: I think it is sufficient if this is calculated only once: full_J_var = pin.computeJointJacobiansTimeVariation( self.model, self.data, q, v) J_var_frame = pin.getJointJacobianTimeVariation( self.model, self.data, joint_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :] dyn_comp = np.matmul(J_var_frame, v) acc = acc - dyn_comp # TODO: I think it is sufficient if this is calculated only once: full_JAC = pin.computeJointJacobians(self.model, self.data, q) J_NEW = pin.getJointJacobian( self.model, self.data, joint_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :] J = J_NEW J_inv = np.matmul(np.linalg.pinv(np.matmul(J.T, J)), J.T) # print (J_inv) torque = np.matmul(M_joint, np.matmul(J_inv, acc)) return self.inv_trafo_q(torque)
def comp_combined_torque(self, q, des_force, des_torque, in_touch, center, p1, p2, p3): P = [p1, p2, p3] # only rotate when completely in touch! if (np.sum(in_touch) != 3): return np.zeros((9)), np.zeros((9)) q = self.trafo_q(q) # TODO: I think it is sufficient if this is calculated only once: full_JAC = pin.computeJointJacobians(self.model, self.data, q) J_trans_inv_mat = np.zeros((3, 3 * 12)) for i in range(3): if (in_touch[i] == 1): idx = i * 4 + 4 idx_low = idx - 4 J_NEW = pin.getJointJacobian( self.model, self.data, idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :] J_trans = J_NEW.T J_trans_inv = np.matmul( np.linalg.pinv(np.matmul(J_trans.T, J_trans)), J_trans.T) # print (np.shape(J_trans_inv)) J_trans_inv = np.matmul(self.skew(P[i] - center), J_trans_inv) # print(np.shape(J_trans_inv)) # input("STOOP") J_trans_inv_mat[:, idx_low * 3:idx * 3] = J_trans_inv add_torque_force = np.matmul( np.matmul( np.linalg.pinv(np.matmul(J_trans_inv_mat.T, J_trans_inv_mat)), J_trans_inv_mat.T), des_torque) add_torque_force1 = self.inv_trafo_q(add_torque_force[:12]) add_torque_force2 = self.inv_trafo_q(add_torque_force[12:24]) add_torque_force3 = self.inv_trafo_q(add_torque_force[24:36]) # print (J_trans_inv_mat) # input ("WAIT....") # net_force = net_force + np.matmul(J_trans_inv, torque) # print (net_force) # input ("NET FORCE") return np.zeros( (9)), (add_torque_force1 + add_torque_force2 + add_torque_force3)
def cartesian_position_control(self, model, data, q, v, location, threshold): q = self.kinematics.trafo_q(q) v = self.kinematics.trafo_q(v) ## Compute Jacobian ## _JAC = pin.computeJointJacobians(self.model, self.data, q) jacobian_list = [] for idx in self.end_effectors_id: #pin.forwardKinematics(self.model, self.data, self.trafo_q(q)) jacobian = pin.getJointJacobian( self.model, self.data, idx, pin.ReferenceFrame.WORLD, )[:3, :] jacobian_list.append(jacobian) #print('index is ',idx,'with jacobian: ',jacobian) ###################### ## Get the Cartesian fingers Pose ## # print('state robot is : ', self.robot_state[2]) # print('stacked robot state:', np.stack(self.robot_state[2], axis=1)) # print('goal state is:', self.goal_target) fingers_xyz = np.stack(self.robot_state[2], axis=1) xyz_error = self.goal_target - fingers_xyz #print('Error in XYZ is:', xyz_error) #################################### ac = np.zeros((9, )) JOINTS = [[0, 3], [4, 7], [8, 11]] for i in range(3): jac = jacobian_list[i] jac = np.linalg.pinv(jac) er = xyz_error[:, i] q_er = jac @ er ac[i * 3:(i + 1) * 3] = q_er[JOINTS[i][0]:JOINTS[i][1]] - np.array( [0.1, 0.1, 0.1]) * v[self.end_effectors_id[i] - 4:self.end_effectors_id[i] - 1] ac = np.clip(ac, -0.36, 0.36) return ac
def comp_effective_force(self, q, torque, in_touch): q = self.trafo_q(q) torque = self.trafo_q(torque) net_force = np.zeros((3)) full_JAC = pin.computeJointJacobians(self.model, self.data, q) for i in range(3): if (in_touch[i] == 1): idx = i * 4 + 4 J_NEW = pin.getJointJacobian( self.model, self.data, idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :] J_trans = J_NEW.T J_trans_inv = np.matmul( np.linalg.pinv(np.matmul(J_trans.T, J_trans)), J_trans.T) net_force = net_force + np.matmul(J_trans_inv, torque) # print (net_force) # input ("NET FORCE") return net_force
da_dq, da_dqn, NUMDIFF_MODIFIER * h) # threshold was 3e-3, is now 2.11e-4 (see assertNumDiff.__doc__) # Check ABA versus RNEA derivatives (with forces) assertNumDiff( inv(data.M) * dtau_dq, -da_dq, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) # Check ABA versus RNEA + forces (no derivatives) del a for i, f in enumerate(fs[:-1]): fs[i] = f * 0 f = fs[-1].vector M = pinocchio.crba(model, data, q).copy() pinocchio.computeJointJacobians(model, data, q) J = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy() b = pinocchio.rnea(model, data, q, v, zero(model.nv)).copy() a = pinocchio.aba(model, data, q, v, tau, fs).copy() assert (absmax(a - (inv(M) * (tau - b + J.T * f))) < 1e-6) tau = pinocchio.rnea(model, data, q, v, a, fs) assert (absmax(tau - (M * a + b - J.T * f)) < 1e-6) # ------------------------------------------------ # ------------------------------------------------ # ------------------------------------------------ # Checking linear acceleration (i.e. ahat = a.linear + w x v = [a + v x (vlinear,0)].linear ) a = rand(model.nv) * 2 - 1 dt = 1e-6
def compute(self, trajectory, trajectory_derivative, duration, initial_angles, dt=0.1, lam=10.0): """ trajectory: A lambda, the trajectory to follow duration: The total duration of the movement dt: Frequence of configuration update lam: Hyper parameter for the optimisation problem to solve at each iteration """ # Use model.getJointId to get the id of joints that we want to control # TODO get the correct joint id_LH = self.model.getJointId("LHand") data = self.model.createData() # Initial configuration of the robot to adjust in function of the position of the system q = self.model.neutralConfiguration for i in range(len(self.joint_names)): q[self.joint_ids[i]] = initial_angles[i] nb_step = int(duration / dt) t = 0 q_s_result = [] for step in range(nb_step): pin.forwardKinematics(self.model, data, q) # Update all jacobians of the robot pin.computeJointJacobians(self.model, data, q) # Get the Jacobians of the joint to control in the world repair J = pin.getJointJacobian(self.model, data, id_LH, pin.ReferenceFrame.LOCAL)[:3, 7:] # Solve the inverse kinematics # 1. calculer l'erreur courante en fonction de la position des organes terminaux # data.oMi[id_du_joint] contient la position et l'orientation de l'organe par rapport au repere monde X = data.oMi[id_LH].translation R = data.oMi[id_LH].rotation #print(trajectory(t)).T Xdes = np.matrix(trajectory(t)).T error = R.T * (X - Xdes) error_norm = np.linalg.norm(error) #print(error_norm) deriv = np.matrix(trajectory_derivative(t)).T v = -np.dot(np.linalg.pinv(J), deriv + lam * error) z = np.matrix(np.zeros((6, 1))) v = np.concatenate((z, v)) # v_sol is the solution of the least square problem # t_deriv = trajectory_derivative(t) # update current configurqtion q = pin.integrate(self.model, q, v * dt) #print(q.T) #print(q[15:20].T) q_s_result.append([float(q[15])] + [float(q[16])] + [float(q[17])] + [float(q[18])] + [float(q[19])] + [float(q[32])]) t += dt # TODO Check returns type and convert to appropriate format for naoqi functions return np.array(q_s_result)
def computeValueAndJacobian(self): model = self.robot.model; data = self.robot.data nj = len(self.joints) for im, measurement in enumerate(self.measurements): # ^ # compute configuration q = q + q # i i off qi = neutral(model) for i in range(nj): imeas = self.measurementIndices[i] imodel = self.modelQIndices[i] qi[imodel] = measurement['joint_states'][imeas] + \ self.variable.q_off[i,0] forwardKinematics(model, data, qi) computeJointJacobians(model, data) # position of the head Th = data.oMi[self.headId] hTc = self.variable.hTc if measurement.has_key('left_gripper'): meas_cTs = measurement['left_gripper'] Tw = data.oMi[self.lwId] wTs = self.variable.lwTls Jw_full = getJointJacobian(model, data, self.lwId, \ ReferenceFrame.LOCAL) left = True else: meas_cTs = measurement['right_gripper'] Tw = data.oMi[self.rwId] wTs = self.variable.rwTrs Jw_full = getJointJacobian(model, data, self.rwId, \ ReferenceFrame.LOCAL) left = False valueSE3 = meas_cTs.inverse()*hTc.inverse()*Th.inverse()*Tw*wTs value = log6(valueSE3) Jlog = Jlog6(valueSE3) self.value[6*im+0:6*im+6] = value.vector.reshape(6,1) # Compute Jacobian Xh = Th.toActionMatrix() Xw_inv = Tw.toActionMatrixInverse() wXs_inv = wTs.toActionMatrixInverse() hXc = hTc.toActionMatrix() sTc = wTs.inverse()*Tw.inverse()*Th*hTc; sXc = sTc.toActionMatrix() # Fill reduced Jacobian matrices Jh_full = getJointJacobian(model, data, self.headId, \ ReferenceFrame.LOCAL) # columns relative to q_off for i,c in enumerate (self.modelVIndices): self.Jh[0:6,i:i+1] = Jh_full[0:6,c:c+1] self.Jw[0:6,i:i+1] = Jw_full[0:6,c:c+1] self.jacobian[6*im+0:6*im+6,0:nj] = \ Jlog.dot(wXs_inv).dot(-Xw_inv.dot(Xh).dot(self.Jh) + self.Jw) # columns relative to hTc self.jacobian[6*im+0:6*im+6,nj:nj+6] = -Jlog.dot(sXc) # columns relative to lwTls and rwTrs if left: self.jacobian[6*im+0:6*im+6,nj+6: nj+12] = Jlog self.jacobian[6*im+0:6*im+6,nj+12:nj+18] = np.zeros((6,6)) else: self.jacobian[6*im+0:6*im+6,nj+6: nj+12] = np.zeros((6,6)) self.jacobian[6*im+0:6*im+6,nj+12:nj+18] = Jlog