def generate_random_trajectory(robot, npts, maxd, randskip=0): # Use to skip configuration. Have no idea how to set random seed # in pinocchio. for i in range(randskip): pinocchio.randomConfiguration(robot.model) ts = [ 0.0, ] qs = [ pinocchio.randomConfiguration(robot.model), ] while len(qs) < npts: qlast = qs[-1] q = pinocchio.randomConfiguration(robot.model) d = pinocchio.distance(robot.model, qlast, q) if d > maxd: q = pinocchio.interpolate(robot.model, qlast, q, maxd / d) qs.append(q) ts.append(ts[-1] + pinocchio.distance(robot.model, qlast, q)) # Compute velocities and accelerations vs = [ np.zeros(robot.model.nv), ] accs = [ np.zeros(robot.model.nv), ] eps = 0.01 for q0, q1, q2 in zip(qs, qs[1:], qs[2:]): qprev = pinocchio.interpolate( robot.model, q0, q1, 1 - eps / pinocchio.distance(robot.model, q0, q1) ) qnext = pinocchio.interpolate( robot.model, q1, q2, eps / pinocchio.distance(robot.model, q1, q2) ) # (qnext - qprev) / eps vs.append(pinocchio.difference(robot.model, qprev, qnext) / eps) # ((qnext - q) - (q - qprev)) / eps^2 accs.append( ( pinocchio.difference(robot.model, q, qnext) - pinocchio.difference(robot.model, qprev, q) ) / (eps ** 2) ) vs.append(np.zeros(robot.model.nv)) accs.append(np.zeros(robot.model.nv)) from toppra.cpp import PiecewisePolyPath return PiecewisePolyPath.constructHermite(qs, vs, ts)
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) # Perform the forward kinematics over the kinematic tree forwardKinematics(model, data, numpy_vector_joint_pos) # 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 joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i]
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
def test_all(self): model = pin.buildSampleModelHumanoidRandom() joint_name = "larm6_joint" joint_id = model.getJointId(joint_name) frame_id = model.addBodyFrame("test_body", joint_id, pin.SE3.Identity(), -1) data = model.createData() model.lowerPositionLimit[:7] = -1. model.upperPositionLimit[:7] = 1. q = pin.randomConfiguration(model) pin.forwardKinematics(model, data, q) R1 = pin.computeJointKinematicRegressor(model, data, joint_id, pin.ReferenceFrame.LOCAL, pin.SE3.Identity()) R2 = pin.computeJointKinematicRegressor(model, data, joint_id, pin.ReferenceFrame.LOCAL) self.assertApprox(R1, R2) R3 = pin.computeFrameKinematicRegressor(model, data, frame_id, pin.ReferenceFrame.LOCAL) self.assertApprox(R1, R3)
def solve(self, M_des, robot, joint_id, q=None): """ Inverse kinematics for specified joint (joint_id) and desired pose M_des (array 4x4) NOTE The code below is adopted from here (01.03.2020): https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_i-inverse-kinematics.html """ oMdes = pinocchio.SE3(M_des[:3, :3], M_des[:3, 3]) if np.all(q == None): q = pinocchio.neutral(robot.model) i = 0 iter_resample = 0 while True: # forward pinocchio.forwardKinematics(robot.model, robot.data, q) # error between desired pose and the current one dMi = oMdes.actInv(robot.data.oMi[joint_id]) err = pinocchio.log(dMi).vector # Termination criteria if norm(err) < self.inv_kin_sol_params.eps: success = True break if i >= self.inv_kin_sol_params.max_iter: if iter_resample <= self.inv_kin_sol_params.max_resample: q = pinocchio.randomConfiguration(robot.model) iter_resample += 1 continue else: success = False break # Jacobian J = pinocchio.computeJointJacobian(robot.model, robot.data, q, joint_id) # P controller (?) # v* =~ A * e v = -J.T.dot( solve( J.dot(J.T) + self.inv_kin_sol_params.damp * np.eye(6), err)) # integrate (in this case only sum) q = pinocchio.integrate(robot.model, q, v * self.inv_kin_sol_params.dt) i += 1 if not success: q = pinocchio.neutral(robot.model) q_arr = np.squeeze(np.array(q)) q_arr = np.mod(np.abs(q_arr), 2 * np.pi) * np.sign(q_arr) mask = np.abs(q_arr) > np.pi # If angle abs(q) is larger than pi, represent angle as the shorter angle inv_sign(q) * (2*pi - abs(q)) # This is to avoid conflicting with angle limits. q_arr[mask] = (-1) * np.sign( q_arr[mask]) * (2 * np.pi - np.abs(q_arr[mask])) return success, q_arr
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.data = self.model.createData() qmax = np.matrix(np.full((self.model.nq, 1), np.pi)) self.q = pin.randomConfiguration(self.model, -qmax, qmax) self.v = np.random.rand(self.model.nv) self.a = np.random.rand(self.model.nv)
def random_configuration(self, project_fn=None): q = pin.randomConfiguration(self.model) # q = np.random.uniform(-np.pi, np.pi, size=(6,)) qw = ConfigurationWrapper(self, q) qw = self.clip(qw, self._clip_bounds[0], self._clip_bounds[1])[1] if project_fn: qw = project_fn(qw) return qw
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 setUp(self): self.rmodel = rmodel = pin.buildSampleModelHumanoid() self.rdata = rmodel.createData() self.q = pin.randomConfiguration(rmodel) self.vq = rand(rmodel.nv) * 2 - 1 self.aq = zero(rmodel.nv) self.dq = rand(rmodel.nv) * 2 - 1 self.precision = 1e-8
def setUp(self): self.rmodel = rmodel = pin.buildSampleModelHumanoid() self.rdata = rmodel.createData() self.q = pin.randomConfiguration(rmodel) self.vq = rand(rmodel.nv)*2-1 self.aq = zero(rmodel.nv) self.dq = rand(rmodel.nv)*2-1 self.precision = 1e-8
def test_basic(self): model = self.model q_ones = np.ones((model.nq)) self.assertFalse(pin.isNormalized(model, q_ones)) self.assertFalse(pin.isNormalized(model, q_ones, 1e-8)) self.assertTrue(pin.isNormalized(model, q_ones, 1e2)) q_rand = np.random.rand((model.nq)) q_rand = pin.normalize(model, q_rand) self.assertTrue(pin.isNormalized(model, q_rand)) self.assertTrue(pin.isNormalized(model, q_rand, 1e-8)) self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.) <= 1e-8) q_next = pin.integrate(model, self.q, np.zeros((model.nv))) self.assertApprox(q_next, self.q) v_diff = pin.difference(model, self.q, q_next) self.assertApprox(v_diff, np.zeros((model.nv))) q_next = pin.integrate(model, self.q, self.v) q_int = pin.interpolate(model, self.q, q_next, 0.5) self.assertApprox(q_int, q_int) value = pin.squaredDistance(model, self.q, self.q) self.assertTrue((value <= 1e-8).all()) dist = pin.distance(model, self.q, self.q) self.assertTrue(dist <= 1e-8) q_neutral = pin.neutral(model) self.assertApprox(q_neutral, q_neutral) q_rand1 = pin.randomConfiguration(model) q_rand2 = pin.randomConfiguration(model, -np.ones((model.nq)), np.ones((model.nq))) self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8)) self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 1e-8))
def test_getters(self): data = self.model.createData() q = pin.randomConfiguration(self.model) v = np.random.rand(self.model.nv) a = np.random.rand(self.model.nv) pin.forwardKinematics(self.model, data, q, v, a) T = pin.updateFramePlacement(self.model, data, self.frame_idx) self.assertApprox(T, data.oMi[self.parent_idx].act(self.frame_placement)) v = pin.getFrameVelocity(self.model, data, self.frame_idx) self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) self.assertApprox( v, data.oMi[self.parent_idx].act(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) self.assertApprox( v, pin.SE3(T.rotation, np.zeros(3)).act( self.frame_placement.actInv(data.v[self.parent_idx]))) a = pin.getFrameAcceleration(self.model, data, self.frame_idx) self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) self.assertApprox( a, data.oMi[self.parent_idx].act(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) self.assertApprox( a, pin.SE3(T.rotation, np.zeros(3)).act( self.frame_placement.actInv(data.a[self.parent_idx]))) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) a = pin.getFrameClassicalAcceleration( self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
def setUp(self): self.rmodel = rmodel = pin.buildSampleModelHumanoid() self.rdata = rmodel.createData() self.rdata_fd = rmodel.createData() self.rmodel.lowerPositionLimit[:3] = -1. self.rmodel.upperPositionLimit[:3] = -1. self.q = pin.randomConfiguration(rmodel) self.vq = rand(rmodel.nv) * 2 - 1 self.precision = 1e-8
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 setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.data = self.model.createData() qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) self.v = np.random.rand(self.model.nv) self.ddq = np.random.rand(self.model.nv) self.fext = [] for _ in range(self.model.njoints): self.fext.append(pin.Force.Random())
def test_regressor_matrix(self): for _ in range(100): angle = pinocchio.randomConfiguration(self.model) velocity = pinocchio.utils.rand(self.model.nv) acceleration = pinocchio.utils.rand(self.model.nv) Y = self.compute_regressor_matrix(angle, velocity, acceleration) theta = self.get_params() other_tau = Y * theta torque = self.inverse_dynamics(angle, velocity, acceleration) assert ((abs(torque - other_tau) <= 1e-9).all())
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.data = self.model.createData() qmax = np.matrix(np.full((self.model.nq,1),np.pi)) self.q = pin.randomConfiguration(self.model,-qmax,qmax) self.v = rand(self.model.nv) self.tau = rand(self.model.nv) self.v0 = zero(self.model.nv) self.tau0 = zero(self.model.nv) self.tolerance = 1e-9 # we compute J on a different self.data self.J = pin.jointJacobian(self.model,self.model.createData(),self.q,self.model.getJointId('lleg6_joint')) self.gamma = zero(6)
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.parent_idx = self.model.getJointId( "rarm2_joint") if self.model.existJointName("rarm2_joint") else ( self.model.njoints - 1) self.frame_name = self.model.names[self.parent_idx] + "_frame" self.frame_placement = pin.SE3.Random() self.frame_type = pin.FrameType.OP_FRAME self.model.addFrame( pin.Frame(self.frame_name, self.parent_idx, 0, self.frame_placement, self.frame_type)) self.frame_idx = self.model.getFrameId(self.frame_name) self.data = self.model.createData() self.q = pin.randomConfiguration(self.model) self.v = np.random.rand((self.model.nv)) self.a = np.random.rand((self.model.nv))
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 setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.data = self.model.createData() qmax = np.matrix(np.full((self.model.nq, 1), np.pi)) self.q = pin.randomConfiguration(self.model, -qmax, qmax) self.v = rand(self.model.nv) self.tau = rand(self.model.nv) self.v0 = zero(self.model.nv) self.tau0 = zero(self.model.nv) self.tolerance = 1e-9 # we compute J on a different self.data self.J = pin.jointJacobian(self.model, self.model.createData(), self.q, self.model.getJointId('lleg6_joint')) self.gamma = zero(6)
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()
def searchContactPosture(self, numberOfContacts, qguess=None, ntrial=100): """ Search (randomly) a contact configuration with a given number of contacts. - number of contacts: between 1 and len(self.contactCandidates). - qguess is a configuration, of dim rmodel.nq. If None, a random configuration is first sampled. - ntrial: number of random trial. If successfull, set self.success to True, and store the contact posture found in self.qcontact. If not sucessfull, set self.success to False. Returns self.success. """ assert 0 < numberOfContacts < len(self.contactCandidates) if qguess is None: qguess = pin.randomConfiguration(self.rmodel) for itrial in range(ntrial): limbs = random.sample(list(self.contactCandidates.values()), numberOfContacts) stones = random.sample(self.terrain, numberOfContacts) constraints = [Constraint(self.rmodel, limb.frameIndex, stone, limb.contactType) for limb, stone in zip(limbs, stones)] self.postureGenerator.run(qguess, constraints) if self.postureGenerator.sucess: # We found a candidate posture, let' s check that it is acceptable. qcandidate = self.postureGenerator.qopt.copy() # computeCollisions check collision among the collision pairs. collision = pin.computeCollisions(self.rmodel, self.rdata, self.collision_model, self.collision_data, qcandidate, True) if not collision: self.qcontact = qcandidate self.success = True return True self.success = False return False
def test_frameBodyRegressor(self): model = pin.buildSampleModelManipulator() JOINT_ID = model.njoints - 1 framePlacement = pin.SE3.Random() FRAME_ID = model.addBodyFrame ("test_body", JOINT_ID, framePlacement, -1) data = model.createData() q = pin.randomConfiguration(model) v = pin.utils.rand(model.nv) a = pin.utils.rand(model.nv) pin.rnea(model,data,q,v,a) f = framePlacement.actInv(data.f[JOINT_ID]) I = framePlacement.actInv(model.inertias[JOINT_ID]) f_regressor = pin.frameBodyRegressor(model,data,FRAME_ID).dot(I.toDynamicParameters()) self.assertApprox(f_regressor, f.vector)
def test_staticRegressor(self): model = pin.buildSampleModelHumanoidRandom() data = model.createData() data_ref = model.createData() model.lowerPositionLimit[:7] = -1. model.upperPositionLimit[:7] = 1. q = pin.randomConfiguration(model) pin.computeStaticRegressor(model, data, q) phi = zero(4 * (model.njoints - 1)) for k in range(1, model.njoints): Y = model.inertias[k] phi[4 * (k - 1)] = Y.mass phi[4 * k - 3:4 * k] = Y.mass * Y.lever static_com_ref = pin.centerOfMass(model, data_ref, q) static_com = data.staticRegressor * phi self.assertApprox(static_com, static_com_ref)
def test_joint_torque_regressor(self): model = pin.buildSampleModelHumanoidRandom() model.lowerPositionLimit[:7] = -1. model.upperPositionLimit[:7] = 1. data = model.createData() data_ref = model.createData() q = pin.randomConfiguration(model) v = pin.utils.rand(model.nv) a = pin.utils.rand(model.nv) pin.rnea(model,data_ref,q,v,a) params = zero(10*(model.njoints-1)) for i in range(1, model.njoints): params[(i-1)*10:i*10] = model.inertias[i].toDynamicParameters() pin.computeJointTorqueRegressor(model,data,q,v,a) tau_regressor = data.jointTorqueRegressor.dot(params) self.assertApprox(tau_regressor, data_ref.tau)
# Loading Talos arm with FF TODO use a biped or quadruped # ----------------------------------------------------------------------------- robot = loadANYmal() robot.model.armature[6:] = 1. qmin = robot.model.lowerPositionLimit qmin[:7] = -1 robot.model.lowerPositionLimit = qmin qmax = robot.model.upperPositionLimit qmax[:7] = 1 robot.model.upperPositionLimit = qmax rmodel = robot.model rdata = rmodel.createData() # ----------------------------------------- q = pinocchio.randomConfiguration(rmodel) v = rand(rmodel.nv) x = m2a(np.concatenate([q, v])) u = m2a(rand(rmodel.nv - 6)) # ------------------------------------------------- np.set_printoptions(linewidth=400, suppress=True) State = StatePinocchio(rmodel) actModel = ActuationModelFreeFloating(State) gains = pinocchio.utils.rand(2) Mref_lf = FramePlacement(rmodel.getFrameId('LF_FOOT'), pinocchio.SE3.Random()) contactModel6 = ContactModel6D(State, Mref_lf, actModel.nu, gains) rmodel.frames[Mref_lf.frame].placement = pinocchio.SE3.Random() contactModel = ContactModelMultiple(State, actModel.nu)
## ## In this short script, we show how to compute the derivatives of the ## forward dynamics, using the algorithms explained in: ## ## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 ## # Create model and data model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded for a the Simple Humanoid model) model.lowerPositionLimit = -np.matrix(np.ones((model.nq, 1))) model.upperPositionLimit = np.matrix(np.ones((model.nq, 1))) q = pin.randomConfiguration(model) # joint configuration v = np.matrix(np.random.rand(model.nv, 1)) # joint velocity tau = np.matrix(np.random.rand(model.nv, 1)) # joint acceleration # Evaluate the derivatives pin.computeABADerivatives(model, data, q, v, tau) # Retrieve the derivatives in data ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector ddq_dtau = data.Minv # Derivatives of the FD w.r.t. the joint acceleration vector
import pinocchio from sys import argv filename = "ur5.urdf" if len(argv)<2 else argv[1] model = pinocchio.buildModelFromUrdf(filename) data = model.createData() q = pinocchio.randomConfiguration(model) print 'q = ', q.T pinocchio.forwardKinematics(model,data,q) for k in range(model.njoints): print("{:<24} : {: .2f} {: .2f} {: .2f}" .format( model.names[k], *data.oMi[k].translation.T.flat ))
def rand(self): q = pinocchio.randomConfiguration(self.model) v = pinocchio.utils.rand(self.nv) return np.concatenate([q, v])
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.data = self.model.createData() qmax = np.matrix(np.full((self.model.nq,1),np.pi)) self.q = pin.randomConfiguration(self.model,-qmax,qmax)
NON_ACTIVE_COLLISION_PAIRS += [i] robot.deactivateCollisionPairs(NON_ACTIVE_COLLISION_PAIRS) dcrba = DCRBA(robot) coriolis = Coriolis(robot) drnea = DRNEA(robot) robot.initDisplay(loadModel=True) #robot.loadDisplayModel("world/pinocchio", "pinocchio", model_path) robot.viewer.gui.setLightingMode('world/floor', 'OFF') #print robot.model Q_MIN = robot.model.lowerPositionLimit Q_MAX = robot.model.upperPositionLimit DQ_MAX = robot.model.velocityLimit TAU_MAX = robot.model.effortLimit q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX) robot.display(q) # Display the robot in Gepetto-Viewer. nq = robot.nq nv = robot.nv v = mat_zeros(robot.nv) dv = mat_zeros(robot.nv) ddq_ub_min = mat_zeros(nq) + 1e10 ddq_lb_max = mat_zeros(nq) - 1e10 M_max = mat_zeros((nv, nv)) - 1e10 M_min = mat_zeros((nv, nv)) + 1e10 h_max = mat_zeros(nv) - 1e10 h_min = mat_zeros(nv) + 1e10 dh_dq_max = mat_zeros((nv, nq)) - 1e10 dh_dq_min = mat_zeros((nv, nq)) + 1e10
## ## In this short script, we show how to compute the derivatives of the ## forward dynamics, using the algorithms explained in: ## ## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 ## # Create model and data model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded for a the Simple Humanoid model) model.lowerPositionLimit = -np.matrix(np.ones((model.nq,1))) model.upperPositionLimit = np.matrix(np.ones((model.nq,1))) q = pin.randomConfiguration(model) # joint configuration v = np.matrix(np.random.rand(model.nv,1)) # joint velocity tau = np.matrix(np.random.rand(model.nv,1)) # joint acceleration # Evaluate the derivatives pin.computeABADerivatives(model,data,q,v,tau) # Retrieve the derivatives in data ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector ddq_dtau = data.Minv # Derivatives of the FD w.r.t. the joint acceleration vector
print "" import os filename = str(os.path.dirname(os.path.abspath(__file__))) mesh_dir = filename + '/../models/romeo' urdf_model_path = mesh_dir + '/urdf/romeo.urdf' vector = se3.StdVec_StdString() vector.extend(item for item in mesh_dir) robot = tsid.RobotWrapper(urdf_model_path, vector, se3.JointModelFreeFlyer(), False) # model = robot.model() model, collision_model, visual_model = se3.buildModelsFromUrdf(urdf_model_path, mesh_dir, se3.JointModelFreeFlyer()) lb = model.lowerPositionLimit lb[0:3] = -10.0*np.matrix(np.ones(3)).transpose() lb[3:7] = -1.0*np.matrix(np.ones(4)).transpose() ub = model.upperPositionLimit ub[0:3] = 10.0*np.matrix(np.ones(3)).transpose() ub[3:7] = 1.0*np.matrix(np.ones(4)).transpose() q = se3.randomConfiguration(model, lb, ub) print q.transpose() # robot.data() data = model.createData() v = np.matrix(np.ones(robot.nv)).transpose() robot.computeAllTerms(data, q, v) print robot.com(data) print "All test is done"
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom() qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) self.v = np.random.rand(self.model.nv)
M_dist = pin.crba(rmodel_dist, rdata_dist, r.q0) print('M_dist - M') print(M_dist[:6, :6] - M[:6, :6]) # save the result as a txt file: rmodel_dist.saveToText(TXT_FILE) if PLOT_DISPARITY: # funny plots # number of configuration to sample N = 5000 biases = np.zeros((N, 3)) for i in range(N): q = pin.randomConfiguration(rmodel) # Base at the origin q[:6] = 0 q[6] = 1 p_bc = pin.centerOfMass(rmodel, rdata, q) p_bc_dist = pin.centerOfMass(rmodel_dist, rdata_dist, q) biases[i, :] = p_bc_dist - p_bc import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D X, Y, Z = biases[:, 0], biases[:, 1], biases[:, 2] # 2D figures plt.figure('y=f(x)') plt.scatter(X, Y, 'rx')