def test_create_datas(self):
collision_data = self.collision_model.createData()
self.assertEqual(len(collision_data.oMg), self.collision_model.ngeoms)
data_2, collision_data_2 = pin.createDatas(self.model, self.collision_model)
self.assertTrue(self.model.check(data_2))
self.assertEqual(len(collision_data_2.oMg), self.collision_model.ngeoms)
def inverse_kinematics(self, x, y, z):
"""Calculate the joint values for a desired cartesian position"""
model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
"/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf"
)
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model)
JOINT_ID = 5
oMdes = pinocchio.SE3(np.eye(3), np.array([x, y, x]))
q = np.array(abel.sort_joints()[5:])
eps = 1e-1
IT_MAX = 1000
DT = 1e-4
damp = 1e-4
i = 0
while True:
pinocchio.forwardKinematics(model, data, q)
dMi = oMdes.actInv(data.oMi[JOINT_ID])
err = pinocchio.log(dMi).vector
if norm(err) < eps:
success = True
break
if i >= IT_MAX:
success = False
break
J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(model, q, v * DT)
if not i % 10:
print('%d: error = %s' % (i, err.T))
i += 1
if success:
print("Convergence achieved!")
else:
print(
"\nWarning: the iterative algorithm has not reached convergence to the desired precision"
)
print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)
point = JointTrajectoryPoint()
point.positions = [
0, 0, 0, 0, 0, q[0], q[1], q[2], q[3], q[4], q[5], q[6], q[7],
q[8], q[9]
]
abel.move_all_joints(point)
self.direct_kinematics()
def _config_robot(self, urdf_file, package_dir):
if self._b_fixed_base:
# Fixed based robot
self._model, self._collision_model, self._visual_model = pin.buildModelsFromUrdf(
urdf_file, package_dir)
self._n_floating = 0
else:
# Floating based robot
self._model, self._collision_model, self._visual_model = pin.buildModelsFromUrdf(
urdf_file, package_dir, pin.JointModelFreeFlyer())
self._n_floating = 6
self._data, self._collision_data, self._visual_data = pin.createDatas(
self._model, self._collision_model, self._visual_model)
self._n_q = self._model.nq
self._n_q_dot = self._model.nv
self._n_a = self._n_q_dot - self._n_floating
passing_idx = 0
for j_id, j_name in enumerate(self._model.names):
if j_name == 'root_joint' or j_name == 'universe':
passing_idx += 1
else:
self._joint_id[j_name] = j_id - passing_idx
for f_id, frame in enumerate(self._model.frames):
if frame.name == 'root_joint' or frame.name == 'universe':
pass
else:
if f_id % 2 == 0:
# Link
link_id = int(f_id / 2 - 1)
self._link_id[frame.name] = link_id
else:
# Joint
pass
assert len(self._joint_id) == self._n_a
self._total_mass = sum(
[inertia.mass for inertia in self._model.inertias])
self._joint_pos_limit = np.stack(
[self._model.lowerPositionLimit, self._model.upperPositionLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
self._joint_vel_limit = np.stack(
[-self._model.velocityLimit, self._model.velocityLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
self._joint_trq_limit = np.stack(
[-self._model.effortLimit, self._model.effortLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
def direct_kinematics(self):
"""
Compute the direct kinematics for left and right hand (end-effector) by using the URDF model
"""
# Create data required by the algorithms
model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
"/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf"
)
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model)
q = np.array(abel.sort_joints()[5:])
pinocchio.forwardKinematics(model, data, q)
for name, oMi in zip(self.model.names, data.oMi):
print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(
name, *oMi.translation.T.flat)))
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = join(pinocchio_model_dir,
"others/robots") if len(argv) < 2 else argv[1]
mesh_dir = model_path
urdf_model_path = join(model_path, "ur_description/urdf/ur5_robot.urdf")
# Load the urdf model
model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
urdf_model_path, mesh_dir)
print('model name: ' + model.name)
# Create data required by the algorithms
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model)
# Sample a random configuration
q = pinocchio.randomConfiguration(model)
print('q: %s' % q.T)
# Perform the forward kinematics over the kinematic tree
pinocchio.forwardKinematics(model, data, q)
# Update Geometry models
pinocchio.updateGeometryPlacements(model, data, collision_model,
collision_data)
pinocchio.updateGeometryPlacements(model, data, visual_model, visual_data)
# Print out the placement of each joint of the kinematic tree
print("\nJoint placements:")
