def fk(self, body_name, q=None, body_offset=np.zeros(3),
update_kinematics=True, rotation_rep='quat'):
"""
Forward kinematics for a joint configuration.
:param body_name: Name of the body
:param q: Joint configuration. If it is not specified, the current
q value of the model is used.
:param body_offset: Body offset.
:param update_kinematics: Update model with the joint configuration.
:param rotation_rep: Representation used to represent the rotation of
the body: 'quat' or 'rpy'
:return: Pose of the requested body.
"""
if body_name == 'Waist' and not self.floating_base:
body_name = 'ROOT'
if q is None:
q = self.q
body_id = self.model.GetBodyId(body_name)
pos = rbdl.CalcBodyToBaseCoordinates(
self.model,
q,
body_id,
body_offset,
update_kinematics=update_kinematics
)
rot = rbdl.CalcBodyWorldOrientation(
self.model,
q,
body_id,
update_kinematics=update_kinematics
).T
if rotation_rep == 'rpy':
orient = np.array(
tf_transformations.euler_from_matrix(
homogeneous_matrix(rot=rot), axes='sxyz'
)
)
elif rotation_rep == 'quat':
orient = tf_transformations.quaternion_from_matrix(
homogeneous_matrix(rot=rot)
)
else:
raise TypeError("Wrong requested rotation representation: %s" %
rotation_rep)
return np.concatenate((orient, pos))
def get_pose(self, link, wrt_link=None, point=(0., 0., 0.)):
"""
Return the pose of the specified link with respect to the other given link.
Args:
link (int, str): unique link id, or name.
wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world, and
if -1 wrt to the base.
point (np.array[float[3]]): position of the point in link's local frame.
Returns:
np.array[float[7]]: pose (position and quaternion expressed as [x,y,z,w])
"""
link = self.get_link_id(link)
point = np.asarray(point)
position = rbdl.CalcBodyToBaseCoordinates(self.model, self._q, link, point, update_kinematics=False)
orientation = rbdl.CalcBodyWorldOrientation(self.model, self._q, link, update_kinematics=False)
orientation = get_quaternion_from_matrix(orientation)
return np.concatenate((position, orientation))
def test_InverseKinematics (self):
""" Checks whether inverse kinematics methods are consistent """
q = np.random.rand (self.model.q_size)
q_res = np.random.rand (self.model.q_size)
qCS_res = np.random.rand (self.model.q_size)
target_body_ids = np.array([self.body_3])
body_points = np.array([np.zeros(3)])
body_points[0][2] = 1.0
target_positions = np.array([np.zeros(3)])
target_positions[0][2] = 1.0
target_positions[0][0] = 2.0
ori_matrix = np.array([[0., 0., -1.], [0., 1., 0.], [1., 0., 0.]])
CS = rbdl.InverseKinematicsConstraintSet()
CS_size = CS.AddPointConstraint (target_body_ids[0],
body_points[0],
target_positions[0]
)
CS.AddOrientationConstraint (self.body_1, ori_matrix)
CS.dlambda = 0.9
CS.max_steps = 2000
CS.step_tol = 1.0e-8
rbdl.InverseKinematics ( self.model, q, target_body_ids,
body_points, target_positions, q_res,
CS.step_tol, CS.dlambda, CS.max_steps
)
rbdl.InverseKinematicsCS ( self.model, q, CS, qCS_res )
res_point = rbdl.CalcBodyToBaseCoordinates(self.model,
q_res, self.body_3, body_points[0])
res_point2 = rbdl.CalcBodyToBaseCoordinates(self.model,
qCS_res, self.body_3, body_points[0])
res_ori = rbdl.CalcBodyWorldOrientation (self.model,
qCS_res, self.body_1)
assert_almost_equal (target_positions[0], res_point)
assert_almost_equal (target_positions[0], res_point2)
assert_almost_equal (ori_matrix, res_ori)
def fk(model, body_name, q=None, body_offset=np.zeros(3),
update_kinematics=True, rotation_rep='quat'):
"""
:param model:
:param body_name:
:param q:
:param body_offset:
:param update_kinematics:
:param rotation_rep:
:return:
"""
body_name = body_name.encode()
if q is None:
q = np.zeros(model.q_size)
body_id = model.GetBodyId(body_name)
pos = rbdl.CalcBodyToBaseCoordinates(model,
q,
body_id,
body_offset,
update_kinematics=update_kinematics)
rot = rbdl.CalcBodyWorldOrientation(model,
q,
body_id,
update_kinematics=update_kinematics).T
if rotation_rep == 'quat':
orient = tf_transformations.quaternion_from_matrix(
homogeneous_matrix(rot=rot))
elif rotation_rep == 'rpy':
orient = np.array(
tf_transformations.euler_from_matrix(homogeneous_matrix(rot=rot),
axes='sxyz'))
else:
return AttributeError(
"Wrong rotation representation %s. Only 'rpy' and 'quat' available." % rotation_rep)
return np.concatenate((orient, pos))
def jointStateCallback(data):
#global variables shared between functions
global pose_des
global vel_des
global q
global q_old
global qd
J = np.zeros((6, 7))
h = 0.01 #dt
K = (2 / h) * np.eye(6) #Stability for < 2/h
EE_LOCAL_POS = np.array((0.0, 0.0, 0.045)) #Local offset
k = 0.001 #Damped least square method
I = np.eye(6)
## JointState msg
## q = [gripper_left_joint, gripper_right_joint, iiwa_joint_1, iiwa_joint_2, iiwa_joint_3, iiwa_joint_4, iiwa_joint_5, iiwa_joint_6, iiwa_joint_7]
q = data.position[2:]
q = np.asarray(q)
q_old = q
qd = data.velocity[2:]
qd = np.asarray(qd)
#End-Effector Position
x = rbdl.CalcBodyToBaseCoordinates(model, q, 7, EE_LOCAL_POS, True) #x,y,z
#Orientation
R = rbdl.CalcBodyWorldOrientation(model, q, 7, True).T
#From rotation matrix to quaternion
quat = quaternion.from_rotation_matrix(R)
#Input as euler angles for orientation
quat_des = quaternion.from_euler_angles(pose_des[0], pose_des[1],
pose_des[2])
#Angular error
err_ang = quat_des * quat.conjugate()
err_ang = quaternion.as_float_array(err_ang)
#Linear error
err_lin = np.array(
([pose_des[3] - x[0], pose_des[4] - x[1], pose_des[5] - x[2]]))
#Total Error
err = np.array(([
err_ang[1], err_ang[2], err_ang[3], err_lin[0], err_lin[1], err_lin[2]
]))
# Jacobian (angular-linear)
rbdl.CalcPointJacobian6D(model, q, 7, EE_LOCAL_POS, J)
# Jacobian Inverse (Damped)
J_inv = J.T.dot(inv(J.dot(J.transpose()) + (k**2) * I))
#CLIK: Closed Loop Inverse Kinematics
q = q_old + h * J_inv.dot(vel_des + K.dot(err))
qd = J_inv.dot(vel_des + K.dot(err))
#rqt_plot for error
pub_error = rospy.Publisher('/iiwa/iiwa_position_controller/error',
Float64,
queue_size=10)
pub_error.publish(norm(err))
#POSITION CONTROLLER
pub = rospy.Publisher('/iiwa/iiwa_position_controller/command',
JointTrajectory,
queue_size=10)
#JointTrajectory msg generation
joints_str = JointTrajectory()
joints_str.header = Header()
joints_str.header.stamp = rospy.Time.now()
joints_str.joint_names = [
'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4',
'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'
]
#I'll use a single waypoint with the q[] values calculated with CLIK
point = JointTrajectoryPoint()
point.positions = [q[0], q[1], q[2], q[3], q[4], q[5], q[6]]
#point.velocities = [ qd[0], qd[1], qd[2], qd[3], qd[4], qd[5], qd[6] ]
point.time_from_start = rospy.Duration(1)
joints_str.points.append(point)
pub.publish(joints_str)
#Gripper Controller
pub_left = rospy.Publisher(
'/iiwa/gripper_left_position_controller/command',
Float64,
queue_size=10)
pub_right = rospy.Publisher(
'/iiwa/gripper_right_position_controller/command',
Float64,
queue_size=10)
#Open Position
pub_left.publish(0)
pub_right.publish(0)
rate.sleep()
q_old = q