def dyn_value(self, t, q, v, update_geometry=False): J_am = self.robot.momentumJacobian(q, v, update_geometry) J_am = J_am[3:, :] L = J_am * v L_ref, dL_ref, ddL_ref = self._ref_traj(t) dL_des = dL_ref - self.kp * (L - L_ref) # Justin's code to compute drift (does not work) #g = self.robot.bias(q,0*v) #b = self.robot.bias(q,v) #b -= g; #com_p = self.robot.com(q) #cXi = SE3.Identity() #oXi = self.robot.data.oMi[1] #cXi.rotation = oXi.rotation #cXi.translation = oXi.translation - com_p #b_com = cXi.actInv(se3.Force(b[:6,0])) #b_com = b_com.angular; # Del Prete's quick and dirty way to compute drift #b_com[:] = 0.0; # compute momentum Jacobian at next time step assuming zero acc dt = 1e-3 q_next = se3.integrate(self.robot.model, q, dt * v) se3.ccrba(self.robot.model, self._data, q_next, v) J_am_next = self._data.Ag[3:, :] b_com = (J_am_next - J_am) * v / dt return J_am[self._mask, :], b_com[self._mask, :], dL_des[self._mask, 0]
def finiteDifferencesdA_dq(model, data, q, v, h): ''' dAi/dq ''' q0 = q.copy() v0 = v.copy() tensor_dAi_dq = np.zeros((6, model.nv, model.nv)) se3.forwardKinematics(model, data, q0, v0) se3.computeJacobians(model, data, q0) pcom_ref = se3.centerOfMass(model, data, q0).copy() se3.ccrba(model, data, q0, v0) A0i = data.Ag.copy() oMc_ref = se3.SE3.Identity() #data.oMi[1].copy() oMc_ref.translation = pcom_ref #oMc_ref.translation - pcom_ref for j in range(model.nv): #vary q vh = np.matrix(np.zeros((model.nv, 1))) vh[j] = h q_integrated = se3.integrate(model, q0.copy(), vh) se3.forwardKinematics(model, data, q_integrated) se3.computeJacobians(model, data, q_integrated) pcom_int = se3.centerOfMass(model, data, q_integrated).copy() se3.ccrba(model, data, q_integrated, v0) A0_int = data.Ag.copy() oMc_int = se3.SE3.Identity() #data.oMi[1].copy() oMc_int.translation = pcom_int #oMc_int.translation - pcom_int cMc_int = oMc_ref.inverse() * oMc_int A0_int = cMc_int.dualAction * A0_int tensor_dAi_dq[:, :, j] = (A0_int - A0i) / h return tensor_dAi_dq
def forward_robot(self, q, dq): # Update the pinocchio model. self.robot.forwardKinematics(q, dq) self.robot.computeJointJacobians(q) self.robot.framesForwardKinematics(q) se3.ccrba(self.robot.model, self.robot.data, q, dq) self.last_q = q.copy() self.last_dq = dq.copy()
def _update_centroidal_quantities(self): pin.ccrba(self._model, self._data, self._q, self._q_dot) self._hg = np.zeros_like(self._data.hg) self._hg[0:3] = np.copy(self._data.hg.angular) self._hg[3:6] = np.copy(self._data.hg.linear) self._Ag = np.zeros_like(self._data.Ag) self._Ag[0:3] = np.copy(self._data.Ag[3:6, :]) self._Ag[3:6] = np.copy(self._data.Ag[0:3, :]) self._Ig = np.zeros_like(self._data.Ig) self._Ig[0:3, 0:3] = np.copy(self._data.Ig)[3:6, 3:6] self._Ig[3:6, 3:6] = np.copy(self._data.Ig)[0:3, 0:3]
def dyn_value(self, t, q, v): #(hg_ref, vhg_ref, ahg_ref) = self._ref_traj(t) vhg_ref = np.matrix([0., 0., 0., 0., 0., 0.]).T model = self.robot.model data = self.robot.data JMom = se3.ccrba(model, data, q, v) hg_prv = data.hg.vector.copy()[self._mask,:] #self.p_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:] #self.v_error = self.robot.fext[self._mask,:] - vhg_ref[self._mask,:] #self.v_error = self.robot.fext[self._mask,:] #self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:] #self.a_des = -self.kv*self.v_error #+model.gravity.vector[self._mask,:] self.a_des = self.kv*self.robot.fext[self._mask,:]#vhg_ref #+model.gravity.vector[self._mask,:] #self.drift = 0 * self.a_des #self.a_des = #*********************** p_com = data.com[0] cXi = SE3.Identity() oXi = self.robot.data.oMi[1] cXi.rotation = oXi.rotation cXi.translation = oXi.translation - p_com m_gravity = model.gravity.copy() model.gravity.setZero() b = se3.nle(model,data,q,v) model.gravity = m_gravity f_ff = se3.Force(b[:6]) f_com = cXi.act(f_ff) hg_drift = f_com.angular self.drift=f_com.np[self._mask,:] #************************ #print JMom.copy()[self._mask,:].shape #print self.__gain_matrix.shape self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix return self._jacobian, self.drift, self.a_des
def dyn_value(self, t, q, v): (self.hg_ref, self.dhg_ref, aahg_ref) = self._ref_traj(t) JMom = se3.ccrba(self.robot.model, self.robot.data, q, v) b = self._getBiais(q,v) self.hg_act = self.robot.data.hg.np.A.copy() self.dhg_act = self._calculateHdot(q) self.drift=b[self._mask,:] self.p_error = self.hg_act[self._mask,:] - self.hg_ref[self._mask,:] self.v_error = self.dhg_act[self._mask,:] - self.dhg_ref[self._mask,:] self.dh_des = - ((self.kp * self.p_error) + (self.kv*self.v_error) ) self.h_des = - (self.kp * self.p_error) self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix #self.drift = 0*self.dh_des return self._jacobian, self.drift, self.h_des
# Get URDF filename script_dir = os.path.dirname(os.path.realpath(__file__)) pinocchio_model_dir = os.path.join(script_dir, "../urdf") urdf_filename = pinocchio_model_dir + "/talos_full_legs_v2.urdf" # Load URDF model model = pinocchio.buildModelFromUrdf(urdf_filename, pinocchio.JointModelFreeFlyer()) data = model.createData() print('model name: ' + model.name) # Normal configuration (every joint is in origin) q = pinocchio.neutral(model) q[7:] = numpy.ndarray((12,), buffer=numpy.array([0.0, 0.028730477193730477, -0.7060605463076763, 1.4774145045100333, \ -0.771353958202357, -0.028730477193733783, 0.0, -0.028730477193724013, -0.7060605463076761, \ 1.4774145045100333, -0.7713539582023572, 0.028730477193724013])) v = pinocchio.utils.zero(model.nv) # Computes the centroidal mapping, the centroidal momentum and the Centroidal Composite # Rigid Body Inertia, puts the result in Data and returns the centroidal mapping. pinocchio.ccrba(model, data, q, v) # Shows the Inertia Matrix print('Inertia') print(data.Ig) # Computes and shows the center of mass print('Centre of mass:') print(pinocchio.centerOfMass(model, data, q))
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
# EXERCIZE 2: Compute robot kinetic energy #get a random generalized velocity v = rand(model.nv) # Update the joint and frame placements pin.forwardKinematics(model,data,q,v) # compute using generalized velocities and system mass matrix # ..... ## compare with pinocchio native function pin.computeKineticEnergy(model,data,q,v) #print "TEST2: ", EkinRobot - data.kinetic_energy ##EXERCISE 3: Build the transformation matrix to use com coordinates # get the location of the base frame w_base = data.oMi[1].translation #compute centroidal quantitities (hg, Ag and Ig) pin.ccrba(model, data, q, v) # G_T_B = ... # EXERCISE 4: Check the mass matrix becomes block diagonal if you appy the tranform # M_g = ... #print "\n The mass matrix expressed at the com becomes diagonal: \n", M_g # EXERSISE 5: Check that joint gravity vector nullifies # Gg = ...
def cam(self, q, vel): hg = [] for i in range(0, len(q)): se3.ccrba(self.model, self.data, q[i], vel[i]) hg.append(self.data.hg.copy()) return hg