Example #1
0
def computeWrench(cs, Robot, dt):
    rp = RosPack()
    urdf = rp.get_path(
        Robot.packageName
    ) + '/urdf/' + Robot.urdfName + Robot.urdfSuffix + '.urdf'
    #srdf = "package://" + package + '/srdf/' +  cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
    robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
                                       pin.JointModelFreeFlyer(), False)
    model = robot.model
    data = robot.data
    q_t = cs.concatenateQtrajectories()
    dq_t = cs.concatenateQtrajectories()
    ddq_t = cs.concatenateQtrajectories()
    t = q_t.min()
    for phase in cs.contactPhases:
        phase.wrench_t = None
        t = phase.timeInitial
        while t <= phase.timeFinal:
            pin.rnea(model, data, phase.q_t(t), phase.dq_t(t), phase.ddq_t(t))
            pcom, vcom, acom = robot.com(phase.q_t(t), phase.dq_t(t),
                                         phase.ddq_t(t))
            # FIXME : why do I need to call com to have the correct values in data ??
            phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
            if phase.wrench_t is None:
                phase.wrench_t = piecewise(
                    polynomial(phi0.vector.reshape(-1, 1), t, t))
            else:
                phase.wrench_t.append(phi0.vector, t)
            t += dt
            if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.:
                t = phase.timeFinal
    def test_coriolis_matrix(self):
        model = self.model

        C = pin.computeCoriolisMatrix(model, self.data, self.q, self.v)

        data2 = model.createData()
        tau_coriolis_ref = pin.rnea(
            model, data2, self.q, self.v, self.a * 0) - pin.rnea(
                model, data2, self.q, self.v * 0, self.a * 0)

        self.assertApprox(tau_coriolis_ref, C.dot(self.v))
Example #3
0
 def inverseDynamics(self, q, v, a, f_ext=None):
     '''ID(q, v, a, f_ext)
      f_ext: Vector of external forces expressed in the local frame of each joint 
      do it recursively
     '''
     if f_ext is None:
         for i in range(0, len(q)):
             Tau.append(se3.rnea(self.model, self.data, q, v, a))
     else:
         for i in range(0, len(q)):
             Tau.append(se3.rnea(self.model, self.data, q, v, a, f_ext))
     return Tau
Example #4
0
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()
Example #5
0
    def test_rnea(self):
        model = self.model
        data = model.createData()

        q = zero(model.nq)
        qdot = zero(model.nv)
        qddot = zero(model.nv)
        for i in range(model.nbody):
            data.a[i] = se3.Motion.Zero()

        se3.rnea(model, data, q, qdot, qddot)
        for i in range(model.nbody):
            self.assertApprox(data.v[i].np, zero(6))
        self.assertApprox(data.a_gf[0].np, -model.gravity.np)
        self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
Example #6
0
    def test_rnea(self):
        model = self.model
        data = model.createData()

        q = zero(model.nq)
        qdot = zero(model.nv)
        qddot = zero(model.nv)
        for i in range(model.nbody):
            data.a[i] = se3.Motion.Zero()

        se3.rnea(model, data, q, qdot, qddot)
        for i in range(model.nbody):
            self.assertApprox(data.v[i].np, zero(6))
        self.assertApprox(data.a_gf[0].np, -model.gravity.np)
        self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
Example #7
0
def forces_from_torques_full(model, data, q, v, dv, tauj, Jlinvel):

    tau_tot = pin.rnea(model, data, q, v, dv)
    tau_forces = tau_tot
    tau_forces[6:] -= tauj
    forces, _, rank, s = np.linalg.lstsq(Jlinvel.T, tau_forces, rcond=None)
    return forces
Example #8
0
def forces_from_torques(model, data, q, v, dv, tauj, Jlinvel_red):

    tau_tot = pin.rnea(model, data, q, v, dv)
    tau_forces = tau_tot[6:] - tauj
    # forces = np.linalg.solve(Jlinvel_red.T, tau_forces)   # equivalent
    forces, _, rank, s = np.linalg.lstsq(Jlinvel_red.T, tau_forces, rcond=None)
    return forces
Example #9
0
 def update(self, q):
     se3.computeAllTerms(self.model, self.data, self.q, self.v)
     #se3.forwardKinematics(self.model, self.data, q, self.v, self.a)
     #- se3::forwardKinematics
     #- se3::crba
     #- se3::nonLinearEffects
     #- se3::computeJacobians
     #- se3::centerOfMass
     #- se3::jacobianCenterOfMass
     #- se3::kineticEnergy
     #- se3::potentialEnergy
     se3.framesKinematics(self.model, self.data, q)
     se3.computeJacobians(self.model, self.data, q)
     se3.rnea(self.model, self.data, q, self.v, self.a)
     self.biais(self.q, self.v)
     self.q = q.copy()
def estimate_forces(robot, q, vq, dvq, tau_j, cf_ids, dim, world_frame=True):
    """
    dim: 3 -> point contact == 3D force, 6 -> planar contact == 6D wrench
    """
    robot.forwardKinematics(q)

    # simplify the assumption:
    # No linear velocity   --> almost no influence
    # vq[0:3] = 0
    # No angular velocity  --> almost no influence
    # vq[3:6] = 0
    # No joint velocity    --> max 1N difference during flight phase and high variation (lag of the est)
    # vq[6:] = 0
    # No linear spatial acceleration  --> import difference on all axes during the whole traj(~ 10 N diff)
    # dvq[:3] = 0
    # No angular spatial acceleration  --> difference spikes (~ 5N when entering flight phase)
    # dvq[3:6] = 0
    # No joint acceleration            --> ~ 10-20 N diference uniquelly during flight phase
    # dvq[6:] = 0

    tau_cf = pin.rnea(rmodel, rdata, q, vq, dvq)
    tau_cf[6:] -= tau_j

    # compute/stack contact jacobians
    Jlinvel = compute_joint_jac(robot, q, cf_ids, dim, world_frame=world_frame)

    forces = np.linalg.pinv(Jlinvel.T) @ tau_cf

    return forces.reshape((len(cf_ids), dim)).T
Example #11
0
def  iden_model(model, data, N, nq, nv, q, v, a): 
	"""This function calculates joint torques and generates the joint torque regressor.
		Note: a flag IsFrictioncld to include in dynamic model
		Input: 	model, data: model and data structure of robot from Pinocchio
				q, v, a: joint's position, velocity, acceleration
				N : number of samples
				nq: length of q
		Output: tau: vector of joint torque
				W : joint torque regressor"""
	tau = np.empty(nq*N)
	W = np.empty([N*nq, 10*nq]) 
	for i in range(N):
		tau_temp = pin.rnea(model, data, q[i,:], v[i,:], a[i,:])
		W_temp = pin.computeJointTorqueRegressor(model, data, q[i,:], v[i,:], a[i,:])
		for j in range(nq):
			tau[j*N + i] = tau_temp[j]
			W[j*N + i, :] = W_temp[j,:]
	if isFrictionincld:
		W = np.c_[W,np.zeros([N*nq,2*nq])]
		for i in range(N):
			for j in range(nq):
				tau[j*N + i] = tau[j*N + i] + v[i,j]*fv + np.sign(v[i,j])*fc
				W[j*N + i, 10*nq+2*j] = v[i,j]
				W[j*N + i, 10*nq+2*j + 1] = np.sign(v[i,j])
	return tau, W
def estimate_forces(q, vq, dvq, tau_j, cf_ids, world_frame=True):
    robot.forwardKinematics(q)

    # simplify the assumption:
    # No linear velocity   --> almost no influence
    # vq[0:3] = 0
    # No angular velocity  --> almost no influence
    # vq[3:6] = 0
    # No joint velocity    --> max 1N difference during flight phase and high variation (lag of the est)
    # vq[6:] = 0
    # No linear spatial acceleration  --> import difference on all axes during the whole traj(~ 10 N diff)
    # dvq[:3] = 0
    # No angular spatial acceleration  --> difference spikes (~ 5N when entering flight phase)
    # dvq[3:6] = 0
    # No joint acceleration            --> ~ 10-20 N diference uniquelly during flight phase
    # dvq[6:] = 0

    tau_cf = pin.rnea(rmodel, rdata, q, vq, dvq)
    tau_cf[6:] -= tau_j

    # compute/stack contact jacobians
    Jlinvel = np.zeros((12, robot.nv))
    for i, frame_id in enumerate(contact_frame_ids):
        if world_frame:
            oTl = robot.framePlacement(q, frame_id, update_kinematics=False)
            Jlinvel[3 * i:3 *
                    (i + 1), :] = oTl.rotation @ robot.computeFrameJacobian(
                        q, frame_id)[:3, :]  # jac in world coord
        else:
            Jlinvel[3 * i:3 * (i + 1), :] = robot.computeFrameJacobian(
                q, frame_id)[:3, :]  # jac in local coord

    forces = np.linalg.pinv(Jlinvel.T) @ tau_cf

    return forces.reshape((4, 3)).T
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)
    computeRNEADerivatives(model, data, numpy_vector_joint_pos,
                           numpy_vector_joint_vel, numpy_vector_joint_acc)
    dtorques_dq = data.dtau_dq
    dtorques_dqd = data.dtau_dv
    dtorques_dqdd = data.M
Example #14
0
def constraint(tau):
    tau = a2m(tau)
    q = rmodel.neutralConfiguration
    v = zero(rmodel.nv)
    a = zero(rmodel.nv)
    res = m2a(pinocchio.rnea(rmodel, rdata, q, v, a) - tau)
    return res.tolist()
Example #15
0
def cost(tau):
    tau = a2m(tau)
    q = rmodel.neutralConfiguration
    v = zero(rmodel.nv)
    a = zero(rmodel.nv)
    res = m2a(pinocchio.rnea(rmodel, rdata, q, v, a) - tau)
    return sum(res**2)
    def test_jointBodyRegressor(self):
        model = pin.buildSampleModelManipulator()
        data = model.createData()

        JOINT_ID = model.njoints - 1

        q = pin.randomConfiguration(model)
        v = pin.utils.rand(model.nv)
        a = pin.utils.rand(model.nv)

        pin.rnea(model,data,q,v,a)

        f = data.f[JOINT_ID]

        f_regressor = pin.jointBodyRegressor(model,data,JOINT_ID).dot(model.inertias[JOINT_ID].toDynamicParameters())

        self.assertApprox(f_regressor, f.vector)
def cid2(q_, v_, tau_):
    pinocchio.computeJointJacobians(model, data, q_)
    K = Kid(q_)
    b = pinocchio.rnea(model, data, q_, v_, zero(model.nv)).copy()
    pinocchio.forwardKinematics(model, data, q_, v_, zero(model.nv))
    gamma = data.a[-1].vector
    r = np.concatenate([tau_ - b, -gamma])
    return inv(K) * r
    def test_generalized_gravity(self):
        model = self.model

        tau = pin.computeGeneralizedGravity(model, self.data, self.q)

        data2 = model.createData()
        tau_ref = pin.rnea(model, data2, self.q, self.v * 0, self.a * 0)

        self.assertApprox(tau, tau_ref)
    def test_nle(self):
        model = self.model

        tau = pin.nonLinearEffects(model, self.data, self.q, self.v)

        data2 = model.createData()
        tau_ref = pin.rnea(model, data2, self.q, self.v, self.a * 0)

        self.assertApprox(tau, tau_ref)
Example #20
0
def forces_from_torques_full(model, data, q, v, dv, tauj, Jlinvel):
    """
    Compromise between the euler equations and contact leg dynamics
    """
    tau_tot = pin.rnea(model, data, q, v, dv)
    tau_forces = tau_tot
    tau_forces[6:] -= tauj
    forces, _, rank, s = np.linalg.lstsq(Jlinvel.T, tau_forces, rcond=None)
    return forces
Example #21
0
def forces_from_torques(model, data, q, v, dv, tauj, Jlinvel_red):
    """
    Works only well if the contact kinematics are perfect
    """
    tau_tot = pin.rnea(model, data, q, v, dv)
    tau_forces = tau_tot[6:] - tauj
    # forces = np.linalg.solve(Jlinvel_red.T, tau_forces)   # equivalent
    forces, _, rank, s = np.linalg.lstsq(Jlinvel_red.T, tau_forces, rcond=None)
    return forces
Example #22
0
 def compute_gravity_compensation(self, robot_state, torque):
     pin.forwardKinematics(self.model, self.data,
                           self.trafo_q(robot_state[0]))
     b = pin.rnea(self.model, self.data, self.trafo_q(robot_state[0]),
                  self.trafo_q(robot_state[1]), np.zeros(12))
     add = self.inv_trafo_q(b)
     torque += add
     torque = np.clip(torque, -0.36, 0.36)
     return torque
def computeWrench(res):
    rp = RosPack()
    urdf = rp.get_path(
        cfg.Robot.packageName
    ) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
    #srdf = "package://" + package + '/srdf/' +  cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
    robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
                                       pin.JointModelFreeFlyer(), False)
    model = robot.model
    data = robot.data
    for k in range(res.N):
        pin.rnea(model, data, res.q_t[:, k], res.dq_t[:, k], res.ddq_t[:, k])
        pcom, vcom, acom = robot.com(
            res.q_t[:, k], res.dq_t[:, k], res.ddq_t[:, k]
        )  # FIXME : why do I need to call com to have the correct values in data ??
        phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
        res.wrench_t[:, k] = phi0.vector
    return res
Example #24
0
def talker():
      rospy.init_node("tocabi_pinocchio", anonymous = False)
      rospy.Subscriber("/tocabi/pinocchio/jointstates", JointState, callback)
      global model
      pub = rospy.Publisher("/tocabi/pinocchio", model, queue_size=1)
      modelmsg.CMM = [0 for i in range(33*6)]
      modelmsg.COR = [0 for i in range(33*33)]
      modelmsg.M = [0 for i in range(33*33)]
      modelmsg.g = [0 for i in range(33)]

      rate = rospy.Rate(1000) #10hz
      model = pinocchio.buildModelFromUrdf("/home/jhk/catkin_ws/src/dyros_tocabi/tocabi_description/robots/dyros_tocabi.urdf",pinocchio.JointModelFreeFlyer())      
      data = model.createData()
      global start
      q = pinocchio.randomConfiguration(model)
      qdot = pinocchio.utils.zero(model.nv)
      qdot_t = pinocchio.utils.zero(model.nv)
      qddot_t = pinocchio.utils.zero(model.nv)

      while not rospy.is_shutdown():
             if start:
                  global qmsg
                  for i in range(0, 39):
                        q[i] = qmsg.position[i]
                        
                  for j in range(0, 38):
                        qdot[j] = qmsg.velocity[j]
                        qdot_t[j] = 0.0
                        qddot_t[j] = 0.0

                  CMM = pinocchio.computeCentroidalMap(model, data, q)
                  pinocchio.crba(model, data, q)
                  pinocchio.computeCoriolisMatrix(model, data, q, qdot)
                  pinocchio.rnea(model, data, q, qdot_t, qddot_t)

                  CMM_slice = CMM[:,6:39]
                  COR_slice = data.C[:,6:39]
                  M_slice = data.M[:,6:39]
                  g_slice = data.tau[6:39]

                  modelmsg.CMM = CMM_slice.flatten()
                  modelmsg.COR = COR_slice.flatten()
                  modelmsg.M = M_slice.flatten()
                  modelmsg.g = g_slice.flatten()
Example #25
0
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_static_torque(self):
        model = self.model

        tau = pin.computeStaticTorque(model, self.data, self.q, self.fext)

        data2 = model.createData()
        tau_ref = pin.rnea(model, data2, self.q, self.v * 0, self.a * 0,
                           self.fext)

        self.assertApprox(tau, tau_ref)
def estimate_torques(robot, q, vq, dvq, forces, cf_ids, dim, world_frame=True):
    robot.forwardKinematics(q)
    tau_tot = pin.rnea(rmodel, rdata, q, vq, dvq)

    # compute/stack contact jacobians
    Jlinvel = compute_joint_jac(robot, q, cf_ids, dim, world_frame=world_frame)

    tau_forces = Jlinvel.T @ forces

    return (tau_tot - tau_forces)[6:]
 def compute_id_torques(self, q, v, a):
     """
     This function computes the torques for the give state using rnea
     Input:
         q : joint positions
         v : joint velocity
         a : joint acceleration
     """
     return np.reshape(pin.rnea(self.pinModel, self.pinData, q, v, a),
                       (self.nv, ))
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
Example #30
0
    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 computeWaistData(robot, res):
    N = len(res.t_t)

    model = robot.model
    data = robot.data

    res.waist_t = np.matrix(np.zeros([3, N]))
    res.waist_orientation_t = np.matrix(np.empty([3, N]))

    for k in range(N):
        q = res.q_t[:, k]
        v = res.dq_t[:, k]
        a = res.ddq_t[:, k]

        pin.rnea(model, data, q, v, a)
        pcom, vcom, acom = robot.com(q, v, a)

        res.waist_t[:, k] = robot.data.oMi[1].translation
        res.waist_orientation_t[:, k] = matrixToRpy(robot.data.oMi[1].rotation)

    return res
                                -err_rh-dJdq_rh[:3],
                                err_CP_u
                                                    ])).reshape(ConstraintsSize,)

    lb = -1000*np.ones(robot.nv)
    ub = 1000*np.ones(robot.nv)

    if i==0:
        qp.init(H, g, A, lb, ub, lbA, ubA, np.array([100]))
    else:
        qp.hotstart(H, g, A, lb, ub, lbA, ubA, np.array([100]))
    sol = np.zeros(robot.nv)
    qp.getPrimalSolution(sol)
    a = np.matrix(sol).T

    se3.rnea(robot.model,robot.data,q,v,a)

    tau_joints = robot.data.tau[6:]



    ## configuration integration
    v += np.matrix(a*dt)
    robot.increment(q,v*dt)
    ## Display new configuration
    robot.display(q)
    display_com_projection(robot,q)
    time.sleep(0.005)


model = se3.Model.BuildEmptyModel()
assert(model.nbody==1 and model.nq==0 and model.nv==0)

model = se3.Model.BuildHumanoidSimple()
nb=28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
assert(model.nbody==nb and model.nq==nb-1+6 and model.nv==nb-1+5)
model.inertias[1] = model.inertias[2]
assert( isapprox(model.inertias[1].np,model.inertias[2].np) )
model.jointPlacements[1] = model.jointPlacements[2]
assert( isapprox(model.jointPlacements[1].np,model.jointPlacements[2].np) )
assert(model.parents[0]==0 and model.parents[1] == 0)
model.parents[2] = model.parents[1]
assert( model.parents[2] == model.parents[1] )
assert(model.names[0] == "universe" )
assert( isapprox(model.gravity.np,np.matrix('0; 0; -9.81; 0; 0; 0')) )

data = model.createData()


q = zero(model.nq)
qdot = zero(model.nv)
qddot = zero(model.nv)
for i in range(model.nbody): data.a[i] = se3.Motion.Zero()

se3.rnea(model,data,q,qdot,qddot)
for i in range(model.nbody):
    assert( isapprox(data.v[i].np,zero(6)) )
assert( isapprox(data.a[0].np,-model.gravity.np) )
assert( isapprox(data.f[-1],model.inertias[-1]*data.a[-1]) )
Example #34
0
    print("Check dM = C+C.T /2 \t\t", norm( Mp - (C+C.transpose(1,0,2)) ))
    print("Check dM = Q+Q.T+Q.T+Q.T /2 \t", norm( Mp - 
                                            (Q+Q.transpose(0,2,1)+Q.transpose(1,0,2)+Q.transpose(2,0,1))/2 ))

    # --- CORIOLIS
    # --- CORIOLIS
    # --- CORIOLIS
    coriolis = Coriolis(robot)
    C = coriolis(q,vq)
    print("Check coriolis \t\t\t",norm(C*vq-rnea0(q,vq)))

    # --- DRNEA
    # --- DRNEA
    # --- DRNEA
    drnea = DRNEA(robot)
    aq    = rand(robot.model.nv)
    R = drnea(q,vq,aq)

    NV = robot.model.nv
    Rd = zero([NV,NV])
    eps = 1e-8
    r0 = pin.rnea(robot.model,robot.data,q,vq,aq).copy()
    for i in range(NV):
        dq = zero(NV); dq[i]=eps
        qdq = pin.integrate(robot.model,q,dq)
        Rd[:,i] = (pin.rnea(robot.model,robot.data,qdq,vq,aq)-r0)/eps
    print("Check drnea    \t\t\t",norm(Rd-R))


    
Example #35
0
    def __call__(self,q,vq,aq):
        robot   = self.robot
        pin.rnea(robot.model,robot.data,q,vq,aq)
        NJ      = robot.model.njoints
        NV      = robot.model.nv
        J       = robot.data.J
        oMi     = robot.data.oMi
        v       = [ (oMi[i]*robot.data.v   [i]).vector           for i in range(NJ) ]
        a       = [ (oMi[i]*robot.data.a_gf[i]).vector           for i in range(NJ) ]
        f       = [ (oMi[i]*robot.data.f   [i]).vector           for i in range(NJ) ]
        Y       = [ (oMi[i]*robot.model.inertias[i]).matrix() for i in range(NJ) ]
        Ycrb    = [ (oMi[i]*robot.data.Ycrb[i])     .matrix() for i in range(NJ) ]
        
        Tkf = [ zero([6,NV]) for i in range(NJ) ]
        Yvx = [ zero([6,6]) for i in range(NJ) ]
        adjf = lambda f: -np.bmat([ [zero([3,3]),skew(f[:3])] , [skew(f[:3]),skew(f[3:])] ])
        
        R = self.R = zero([NV,NV])

        for i in reversed(range(1,NJ)):           # i is the torque index :    dtau_i = R[i,:] dq
            i0,i1 = iv(i)[0],iv(i)[-1]+1
            Yi = Y[i]
            Yci = Ycrb[i]
            Si = J[:,i0:i1]
            vi = v[i]
            ai = a[i]
            fi = f[i]
            aqi = aq[i0:i1]
            vqi = vq[i0:i1]
            dvi = Si*vqi
            li = parent(i)

            Yvx[ i] += Y[i]*adj(v[i])
            Yvx[ i] -= adjf(Y[i]*v[i])
            Yvx[ i] -= adjdual(v[i])*Yi

            Yvx[li] += Yvx[i]

            for k in ancestors(i):      # k is the derivative index: dtau   = R[:,k] dq_k
                k0,k1 = iv(k)[0],iv(k)[-1]+1
                Sk = J[:,k0:k1]
                lk = parent(k)

                # Si' Yi Tk ai = Si' Yi ( Sk x (ai-alk) + (vi-vlk) x (Sk x vlk) )
                # Tk Si' Yi ai = Si' Yi ( - Sk x a[lk]  + (vi-vlk) x (Sk x vlk) ) 
                Tkf = Yci*(- MCross(Sk,a[lk]) + MCross(MCross(Sk,v[lk]),v[lk]))
        
                # Tk Si' fs = Tk Si' Ycrb[i] ai + Si' Ys (vs-vi) x (Sk x vlk)
                #           =      ""           + Si' Ys vs x (Sk x vlk) - Si' Ys vi x (Sk x vlk)
                Tkf += Yvx[i]*MCross(Sk,v[lk])

                R[i0:i1,k0:k1]      = Si.T*Tkf
                if i==k:
                    for kk in ancestors(k)[:-1]:
                        kk0,kk1 = iv(kk)[0],iv(kk)[-1]+1
                        Skk = J[:,kk0:kk1]
                        R[kk0:kk1,k0:k1]   =  Skk.T * FCross(Sk,f[i])
                        R[kk0:kk1,k0:k1]   += Skk.T * Tkf

        self.a = a
        self.v = v
        self.f = f
        self.Y = Y
        self.Ycrb = Ycrb
        self.Yvx = Yvx

        return R