def compute_ik(self, position, orientation, qinit=None, numerical=True):
"""
Inverse kinematics
computes joints for robot arm to desired end-effector pose
(w.r.t. 'ARM_BASE_FRAME').
:param position: end effector position (shape: :math:`[3,]`)
:param orientation: end effector orientation.
It can be quaternion ([x,y,z,w],
shape: :math:`[4,]`),
euler angles (yaw, pitch, roll
angles (shape: :math:`[3,]`)),
or rotation matrix (shape: :math:`[3, 3]`)
:param qinit: initial joint positions for numerical IK
:param numerical: use numerical IK or analytical IK
:type position: np.ndarray
:type orientation: np.ndarray
:type qinit: list
:type numerical: bool
:return: None or joint positions
:rtype: np.ndarray
"""
position = position.flatten()
if orientation.size == 4:
quat = orientation.flatten()
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
else:
raise TypeError("Orientation must be in one "
"of the following forms:"
"rotation matrix, euler angles, or quaternion")
# This is needed to conform compute_ik to core.py's compute ik
quat_world_to_base_link = np.asarray(
self.arm_base_link.get_quaternion())
quat = self._quaternion_multiply(quat_world_to_base_link, quat)
rot_world_to_base_link = np.transpose(
prutil.quat_to_rot_mat(
np.asarray(self.arm_base_link.get_quaternion())))
position = np.matmul(position, rot_world_to_base_link)
position = position + np.asarray(self.arm_base_link.get_position())
print(position, prutil.quat_to_rot_mat(quat))
# print(position, quat)
try:
joint_angles = self.arm.solve_ik(position.tolist(),
euler=None,
quaternion=quat.tolist())
except IKError as error:
print(error)
return None
joint_angles = np.asarray(joint_angles)
return joint_angles
def compute_fk_position(self, joint_positions, des_frame):
"""
Given joint angles, compute the pose of desired_frame with respect
to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame
must be in self.arm_link_names
:param joint_positions: joint angles
:param des_frame: desired frame
:type joint_positions: np.ndarray
:type des_frame: string
:return: translational vector and rotational matrix
:rtype: np.ndarray, np.ndarray
"""
joint_positions = joint_positions.flatten()
req = FkCommandRequest()
req.joint_angles = list(joint_positions)
req.end_frame = des_frame
try:
resp = self._fk_service(req)
except:
rospy.logerr("FK Service call failed")
resp = FkCommandResponse()
resp.success = False
if not resp.success:
return None
pos = np.asarray(resp.pos).reshape(3, 1)
rot = prutil.quat_to_rot_mat(resp.quat)
return pos, rot
def get_transform(self, src_frame, dest_frame):
"""
Return the transform from the src_frame to dest_frame
:param src_frame: source frame
:param dest_frame: destination frame
:type src_frame: string
:type dest_frame: basestring
:return:
tuple (trans, rot_mat, quat )
trans: translational vector (shape: :math:`[3, 1]`)
rot_mat: rotational matrix (shape: :math:`[3, 3]`)
quat: rotational matrix in the form
of quaternion (shape: :math:`[4,]`)
:rtype: tuple or ROS PoseStamped
"""
trans, quat = prutil.get_tf_transform(self.tf_listener, src_frame,
dest_frame)
rot_mat = prutil.quat_to_rot_mat(quat)
trans = np.array(trans).reshape(-1, 1)
rot_mat = np.array(rot_mat)
quat = np.array(quat)
return trans, rot_mat, quat
def get_ee_pose(self, base_frame=None):
"""
Return the end effector pose with respect to the base_frame
:param base_frame: reference frame
:type base_frame: string
:return:
tuple (trans, rot_mat, quat)
trans: translational vector (shape: :math:`[3, 1]`)
rot_mat: rotational matrix (shape: :math:`[3, 3]`)
quat: rotational matrix in the form
of quaternion (shape: :math:`[4,]`)
:rtype: tuple or ROS PoseStamped
"""
# raise NotImplementedError
pos = (self.ee_link.get_position(), )
quat = self.ee_link.get_quaternion()
pos = np.asarray(pos).flatten()
quat = np.asarray(quat).flatten()
rot = prutil.quat_to_rot_mat(quat)
return pos, rot, quat
def pose_ee(self):
"""
Return the end effector pose w.r.t 'ARM_BASE_FRAME'
:return:
trans: translational vector (shape: :math:`[3, 1]`)
rot_mat: rotational matrix (shape: :math:`[3, 3]`)
quat: rotational matrix in the form
of quaternion (shape: :math:`[4,]`)
:rtype: tuple (trans, rot_mat, quat)
"""
pos = (self.ee_link.get_position(self.arm_base_link), )
quat = self.ee_link.get_quaternion(self.arm_base_link)
# print("#################Debug start########################")
# print(quat)
# print("#################Debug end########################")
pos = np.asarray(pos).flatten()
quat = np.asarray(quat).flatten()
rot = prutil.quat_to_rot_mat(quat)
return pos, rot, quat
def get_link_transform(self, src, tgt):
"""
Returns the latest transformation from the
target_frame to the source frame,
i.e., the transform of source frame w.r.t
target frame. If the returned
transform is applied to data, it will transform
data in the source_frame into
the target_frame
For more information, please refer to
http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms
:param src: source frame
:param tgt: target frame
:type src: string
:type tgt: string
:returns: tuple(trans, rot, T)
trans: translational vector (shape: :math:`[3,]`)
rot: rotation matrix (shape: :math:`[3, 3]`)
T: transofrmation matrix (shape: :math:`[4, 4]`)
:rtype: tuple(np.ndarray, np.ndarray, np.ndarray)
"""
trans, quat = prutil.get_tf_transform(self._tf_listener, tgt, src)
rot = prutil.quat_to_rot_mat(quat)
T = np.eye(4)
T[:3, :3] = rot
T[:3, 3] = trans
return trans, rot, T
def compute_fk_position(self, joint_positions):
"""
Given joint angles, compute the pose of desired_frame with respect
to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame
must be in self.arm_link_names
:param joint_positions: joint angles
:param des_frame: desired frame
:type joint_positions: np.ndarray
:type des_frame: string
:return: translational vector and rotational matrix
:rtype: np.ndarray, np.ndarray
"""
# raise NotImplementedError
pos, quat = self.ee_link.get_position(), self.ee_link.get_quaternion()
pos = np.asarray(pos)
rot = prutil.quat_to_rot_mat(quat)
return pos, rot
def _rot_matrix(self, habitat_quat):
quat_list = [habitat_quat.x, habitat_quat.y, habitat_quat.z, habitat_quat.w]
return prutil.quat_to_rot_mat(quat_list)
def set_ee_pose(self,
position,
orientation,
plan=True,
wait=True,
numerical=True,
**kwargs):
"""
Commands robot arm to desired end-effector pose
(w.r.t. 'ARM_BASE_FRAME').
Computes IK solution in joint space and calls set_joint_positions.
Will wait for command to complete if wait is set to True.
:param position: position of the end effector (shape: :math:`[3,]`)
:param orientation: orientation of the end effector
(can be rotation matrix, euler angles (yaw,
pitch, roll), or quaternion)
(shape: :math:`[3, 3]`, :math:`[3,]`
or :math:`[4,]`)
The convention of the Euler angles here
is z-y'-x' (intrinsic rotations),
which is one type of Tait-Bryan angles.
:param plan: use moveit the plan a path to move to the desired pose
:param wait: wait until the desired pose is achieved
:param numerical: use numerical inverse kinematics solver or
analytical inverse kinematics solver
:type position: np.ndarray
:type orientation: np.ndarray
:type plan: bool
:type wait: bool
:type numerical: bool
:return: Returns True if command succeeded, False otherwise
:rtype: bool
"""
if orientation.size == 4:
quat = orientation.flatten()
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
quat_world_to_base_link = np.asarray(
self.arm_base_link.get_quaternion())
quat = self._quaternion_multiply(quat_world_to_base_link, quat)
rot_world_to_base_link = np.transpose(
prutil.quat_to_rot_mat(
np.asarray(self.arm_base_link.get_quaternion())))
position = np.matmul(position, rot_world_to_base_link)
position = position + np.asarray(self.arm_base_link.get_position())
try:
arm_path = self.arm.get_path(position.tolist(),
quaternion=quat.tolist(),
ignore_collisions=False)
except ConfigurationPathError as error:
print(error)
return False
# arm_path.visualize()
# done = False
# while not done:
# done = arm_path.step()
# pr.step()
# arm_path.clear_visualization()
arm_path.set_to_end()
return True
