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 make_plan_pose(
self, position, orientation, wait=True, numerical=True, **kwargs
):
if not self.use_moveit:
raise ValueError(
"Using plan=True when moveit is not"
" initialized, did you pass "
"in use_moveit=True?"
)
pose = Pose()
position = position.flatten()
if orientation.size == 4:
orientation = orientation.flatten()
ori_x = orientation[0]
ori_y = orientation[1]
ori_z = orientation[2]
ori_w = orientation[3]
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
else:
raise TypeError(
"Orientation must be in one "
"of the following forms:"
"rotation matrix, euler angles, or quaternion"
)
pose.position.x, pose.position.y, pose.position.z = (
position[0],
position[1],
position[2],
)
(
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
) = (ori_x, ori_y, ori_z, ori_w)
result = self.moveit_group.motionPlanToPose(pose, wait=True)
return [p.positions for p in result]
def test_ee_pose_control(create_robot, position, orientation, numerical):
bot = create_robot
bot.arm.go_home()
time.sleep(1)
bot.arm.set_ee_pose(position=position,
orientation=orientation,
numerical=numerical)
time.sleep(1)
trans, rot, quat = bot.arm.pose_ee
pos_error = np.linalg.norm(position.flatten() - trans.flatten())
if orientation.size == 4:
tgt_quat = orientation.flatten()
elif orientation.size == 3:
tgt_quat = prutil.euler_to_quat(orientation)
elif orientation.size == 9:
tgt_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")
quat_diff = tft.quaternion_multiply(tft.quaternion_inverse(tgt_quat), quat)
rot_similarity = quat_diff[3]
assert rot_similarity > 0.98 and pos_error < 0.02
def compute_ik(self, position, orientation, qinit=None, numerical=True):
"""
Inverse kinematics
: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:
orientation = orientation.flatten()
ori_x = orientation[0]
ori_y = orientation[1]
ori_z = orientation[2]
ori_w = orientation[3]
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
else:
raise TypeError("Orientation must be in one "
"of the following forms:"
"rotation matrix, euler angles, or quaternion")
if qinit is None:
qinit = self.get_joint_angles().tolist()
elif isinstance(qinit, np.ndarray):
qinit = qinit.flatten().tolist()
if numerical:
pos_tol = self.configs.ARM.IK_POSITION_TOLERANCE
ori_tol = self.configs.ARM.IK_ORIENTATION_TOLERANCE
req = IkCommandRequest()
req.init_joint_positions = qinit
req.pose = [
position[0],
position[1],
position[2],
ori_x,
ori_y,
ori_z,
ori_w,
]
req.tolerance = 3 * [pos_tol] + 3 * [ori_tol]
try:
resp = self._ik_service(req)
except:
rospy.logerr("IK Service call failed")
resp = IkCommandResponse()
resp.success = False
if not resp.success:
joint_positions = None
else:
joint_positions = resp.joint_positions
else:
if not hasattr(self, "ana_ik_solver"):
raise TypeError("Analytical solver not provided, "
"please use `numerical=True`"
"to use the numerical method "
"for inverse kinematics")
else:
joint_positions = self.ana_ik_solver.get_ik(
qinit,
position[0],
position[1],
position[2],
ori_x,
ori_y,
ori_z,
ori_w,
)
if joint_positions is None:
return None
print(joint_positions)
return np.array(joint_positions)
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
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 plan:
if not self.use_moveit:
raise ValueError(
"Using plan=True when moveit is not"
" initialized, did you pass "
"in use_moveit=True?"
)
pose = Pose()
position = position.flatten()
if orientation.size == 4:
orientation = orientation.flatten()
ori_x = orientation[0]
ori_y = orientation[1]
ori_z = orientation[2]
ori_w = orientation[3]
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
else:
raise TypeError(
"Orientation must be in one "
"of the following forms:"
"rotation matrix, euler angles, or quaternion"
)
pose.position.x, pose.position.y, pose.position.z = (
position[0],
position[1],
position[2],
)
(
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
) = (ori_x, ori_y, ori_z, ori_w)
result = self.moveit_group.moveToPose(pose, wait=True)
else:
joint_positions = self.compute_ik(
position, orientation, numerical=numerical
)
result = False
if joint_positions is None:
rospy.logerr("No IK solution found; check if target_pose is valid")
else:
result = self.set_joint_positions(joint_positions, plan=plan, wait=wait)
return result
