def __init__(self, model, pinocchioData): nc, nv = model.nimpulse, model.nv self.pinocchio = pinocchioData self.J = np.zeros([nc, nv]) self.Jq = np.zeros([nc, nv]) self.f = np.nan # not set at construction type self.forces = pinocchio.StdVec_Force() for i in range(model.pinocchio.njoints): self.forces.append(pinocchio.Force.Zero()) self.Vq = np.zeros([nc, nv])
def compute_efforts_from_fixed_body( robot: jiminy.Model, position: np.ndarray, velocity: np.ndarray, acceleration: np.ndarray, fixed_body_name: str, use_theoretical_model: bool = True) -> Tuple[np.ndarray, pin.Force]: """Compute the efforts using RNEA method. .. warning:: This function modifies the internal robot data. :param robot: Jiminy robot. :param position: Robot configuration vector. :param velocity: Robot velocity vector. :param acceleration: Robot acceleration vector. :param fixed_body_name: Name of the body frame. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching the robot's state. Optional: True by default. :returns: articular efforts and external forces. """ # Pick the right pinocchio model and data if use_theoretical_model: pnc_model = robot.pinocchio_model_th pnc_data = robot.pinocchio_data_th else: pnc_model = robot.pinocchio_model pnc_data = robot.pinocchio_data # Apply a first run of rnea without explicit external forces # Note that `computeJointJacobians` also compute the forward kinematics # of the model (position only obviously). pin.computeJointJacobians(pnc_model, pnc_data, position) jiminy.rnea(pnc_model, pnc_data, position, velocity, acceleration) # Initialize vector of exterior forces to zero f_ext = pin.StdVec_Force() f_ext.extend(len(pnc_model.names) * (pin.Force.Zero(),)) # Compute the force at the contact frame support_foot_idx = pnc_model.frames[ pnc_model.getBodyId(fixed_body_name)].parent f_ext[support_foot_idx] = pnc_data.oMi[support_foot_idx] \ .actInv(pnc_data.oMi[1]).act(pnc_data.f[1]) # Recompute the efforts with RNEA and the correct external forces tau = jiminy.rnea( pnc_model, pnc_data, position, velocity, acceleration, f_ext) f_ext = f_ext[support_foot_idx] return tau, f_ext
def __init__(self, model, pinocchioData): nc, nv, ndx = model.ncontact, model.nv, model.ndx self.pinocchio = pinocchioData self.J = np.zeros([nc, nv]) self.a0 = np.zeros(nc) self.Ax = np.zeros([nc, ndx]) self.Aq = self.Ax[:, :nv] self.Av = self.Ax[:, nv:] self.f = np.nan # not set at construction type self.forces = pinocchio.StdVec_Force() for i in range(model.pinocchio.njoints): self.forces.append(pinocchio.Force.Zero())
def compute_efforts(trajectory_data, index=(0, 0)): """ @brief Compute the efforts in the trajectory using RNEA method. @param trajectory_data Sequence of States for which to compute the efforts. @param index Index under which the efforts will be saved in the trajectory_data (usually the couple of (roll, pitch) angles of the support foot). """ jiminy_model = trajectory_data['jiminy_model'] use_theoretical_model = trajectory_data['use_theoretical_model'] if use_theoretical_model: pnc_model = jiminy_model.pinocchio_model_th pnc_data = jiminy_model.pinocchio_data_th else: pnc_model = jiminy_model.pinocchio_model pnc_data = jiminy_model.pinocchio_data ## Compute the efforts at each time step root_joint_idx = pnc_model.getJointId('root_joint') for s in trajectory_data['evolution_robot']: # Apply a first run of rnea without explicit external forces pnc.computeJointJacobians(pnc_model, pnc_data, s.q) pnc.rnea(pnc_model, pnc_data, s.q, s.v, s.a) # Initialize vector of exterior forces to 0 fs = pnc.StdVec_Force() fs.extend(len(pnc_model.names) * (pnc.Force.Zero(), )) # Compute the force at the henkle level support_foot_idx = pnc_model.frames[pnc_model.getBodyId( s.support_foot)].parent fs[support_foot_idx] = pnc_data.oMi[support_foot_idx]\ .actInv(pnc_data.oMi[root_joint_idx]).act(pnc_data.f[root_joint_idx]) # Recompute the efforts with RNEA and the correct external forces s.tau[index] = pnc.rnea(pnc_model, pnc_data, s.q, s.v, s.a, fs) s.f_ext[index] = fs[support_foot_idx].copy() # Add the force to the structure s.f[index] = dict( list(zip(pnc_model.names, [f_ind.copy() for f_ind in pnc_data.f]))) # Add the external force applied at the soles as if the floor was a parent of the soles for joint in ['LeftSole', 'RightSole']: ha_M_s = pnc_model.frames[pnc_model.getBodyId(joint)].placement if s.support_foot == joint: s.f[index][joint] = ha_M_s.actInv(s.f_ext[index]) else: s.f[index][joint] = pnc.Force.Zero()
def compute_efforts_from_fixed_body(robot, position, velocity, acceleration, fixed_body_name, use_theoretical_model=True): """ @brief Compute the efforts using RNEA method. @note This function modifies internal data. @param robot The jiminy robot @param position Joint position vector @param velocity See position @param acceleration See position @param fixed_body_name The name of the body frame on which to apply the external forces """ if use_theoretical_model: pnc_model = robot.pinocchio_model_th pnc_data = robot.pinocchio_data_th else: pnc_model = robot.pinocchio_model pnc_data = robot.pinocchio_data # Apply a first run of rnea without explicit external forces pin.computeJointJacobians(pnc_model, pnc_data, position) pin.rnea(pnc_model, pnc_data, position, velocity, acceleration) # Initialize vector of exterior forces to zero f_ext = pin.StdVec_Force() f_ext.extend(len(pnc_model.names) * (pin.Force.Zero(), )) # Compute the force at the contact frame support_foot_idx = pnc_model.frames[pnc_model.getBodyId( fixed_body_name)].parent f_ext[support_foot_idx] = pnc_data.oMi[support_foot_idx] \ .actInv(pnc_data.oMi[1]).act(pnc_data.f[1]) # Recompute the efforts with RNEA and the correct external forces tau = pin.rnea(pnc_model, pnc_data, position, velocity, acceleration, f_ext) f_ext = f_ext[support_foot_idx] return tau, f_ext
def test_aba(self): model = self.model ddq = pin.aba(self.model, self.data, self.q, self.v, self.ddq) null_fext = pin.StdVec_Force() for _ in range(model.njoints): null_fext.append(pin.Force.Zero()) ddq_null_fext = pin.aba(self.model, self.data, self.q, self.v, self.ddq, null_fext) self.assertApprox(ddq_null_fext, ddq) null_fext_list = [] for f in null_fext: null_fext_list.append(f) print('size:', len(null_fext_list)) ddq_null_fext_list = pin.aba(self.model, self.data, self.q, self.v, self.ddq, null_fext_list) self.assertApprox(ddq_null_fext_list, ddq)
def test_rnea(self): model = self.model tau = pin.rnea(self.model, self.data, self.q, self.v, self.a) null_fext = pin.StdVec_Force() for k in range(model.njoints): null_fext.append(pin.Force.Zero()) tau_null_fext = pin.rnea(self.model, self.data, self.q, self.v, self.a, null_fext) self.assertApprox(tau_null_fext, tau) null_fext_list = [] for f in null_fext: null_fext_list.append(f) print('size:', len(null_fext_list)) tau_null_fext_list = pin.rnea(self.model, self.data, self.q, self.v, self.a, null_fext_list) self.assertApprox(tau_null_fext_list, tau)
def __init__(self, model, pinocchioData): # a0: Desired spatial contact acceleration in frame coordinate # x = (q, v) state formed by the configuration point q and its tangent vector v and is described by a nx tuple # nu: dim of control vector # u (tau, lambda) the control input (input torque command and contact forces) # df_dx is really jacobian of dlambda/dx (partial # ddv/dx jacovian of dvel/dx (acceleration) # v_free is the unconbtrained acceleration nc = model.ncontact nv = model.nv ndx = model.ndx # dim of the state vector self.pinocchio = pinocchioData self.J = np.zeros([nc, nv]) self.a0 = np.zeros(nc) self.Ax = np.zeros([nc, ndx]) self.Aq = self.Ax[:, :nv] self.Av = self.Ax[:, nv:] self.f = np.nan # not set at construction type self.forces = pinocchio.StdVec_Force() for i in range(model.pinocchio.njoints): self.forces.append(pinocchio.Force.Zero())
else: m = pin.buildModelFromUrdf(dirpath+"/../model/urdf/ur3e.urdf") d = m.createData() q = pin.utils.zero(m.nq) v = pin.utils.zero(m.nv) u = pin.utils.zero(m.nv) # Random states and control q = pin.utils.rand(m.nq) if use_free_jnt: q[3:7] = q[3:7]/np.linalg.norm(q[3:7]) # normalize quaternion if free joint v = pin.utils.rand(m.nv) u = pin.utils.rand(m.nv) # Set the external forces fext = pin.StdVec_Force() for k in range(m.njoints): fext.append(pin.Force.Zero()) # Evaluate all terms and forward dynamics pin.computeAllTerms(m, d, q, v) a = pin.aba(m, d, q, v, u, fext) # Print state and controls print("pos =", q) print("vel =", v) print("tau =", u) print("acc =", a) print("nle =", d.nle) # Evaluate the ABA derivatives
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) # Se ponen las posiciones, velocidades, aceleraciones y fuerzas (en el # marco de referencia de cada articulación) q = pinocchio.utils.zero(model.nq) v = pinocchio.utils.zero(model.nv) a = pinocchio.utils.zero(model.nv) fext = pinocchio.StdVec_Force() for _ in range(model.njoints): fext.append(pinocchio.Force.Zero()) # Valores de prueba v[12] = 1 a[12] = 10 # Calcula las derivadas de la dinámica # pinocchio.computeABADerivatives(model, data, q, v, a, fext) # Muestra las aceleraciones print('ddq: ' + str(data.ddq)) # Muestra las derivadas de la aceleración articular con respecto a la q print('ddq_dq: ' + str(data.ddq_dq)) # Muestra las derivadas de la aceleración articular con respecto a la velocidad articular
assertNumDiff(dtau_dq, data.dtau_dq, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) # Check RNEA versus ABA derivatives. Mi = pinocchio.computeMinverse(model, data, q) assert (absmax(Mi - inv(data.M)) < 1e-6) D = np.dot(Mi, data.dtau_dq) assertNumDiff(D, -data.ddq_dq, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) # ---- ABA AND RNEA with forces # Set forces container fs = pinocchio.StdVec_Force() for i in range(model.njoints): fs.append(pinocchio.Force.Random()) # Check RNEA derivatives versus finite-diff (with forces) a = pinocchio.aba(model, data, q, v, tau, fs) dtau_dqn = df_dq(model, lambda q_: pinocchio.rnea(model, data, q_, v, a, fs), q) pinocchio.computeRNEADerivatives(model, data, q, v, a, fs) dtau_dq = data.dtau_dq.copy() assertNumDiff(dtau_dqn, dtau_dq, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) # Check ABA derivatives versus finite diff (with forces) da_dqn = df_dq(model, lambda q_: pinocchio.aba(model, data, q_, v, tau, fs), q) pinocchio.computeABADerivatives(model, data, q, v, tau, fs) da_dq = data.ddq_dq.copy()