def compute_ik(self, pos, ori=None, ns=False, *args, **kwargs):
"""
Compute the inverse kinematics solution given the
position and orientation of the end effector.
Args:
pos (list or np.ndarray): position (shape: :math:`[3,]`).
ori (list or np.ndarray): orientation. It can be euler angles
([roll, pitch, yaw], shape: :math:`[3,]`), or
quaternion ([qx, qy, qz, qw], shape: :math:`[4,]`),
or rotation matrix (shape: :math:`[3, 3]`).
ns (bool): whether to use the nullspace options in pybullet,
True if nullspace should be used. Defaults to False.
Returns:
list: solution to inverse kinematics, joint angles which achieve
the specified EE pose (shape: :math:`[DOF]`).
"""
kwargs.setdefault('jointDamping', self._ik_jds)
if ns:
kwargs.setdefault('lowerLimits', self.jnt_lower_limits)
kwargs.setdefault('upperLimits', self.jnt_upper_limits)
kwargs.setdefault('jointRanges', self.jnt_ranges)
kwargs.setdefault('restPoses', self.jnt_rest_poses)
if ori is not None:
ori = arutil.to_quat(ori)
jnt_poss = self._pb.calculateInverseKinematics(
self.robot_id, self.ee_link_id, pos, ori, **kwargs)
else:
jnt_poss = self._pb.calculateInverseKinematics(
self.robot_id, self.ee_link_id, pos, **kwargs)
jnt_poss = list(map(arutil.ang_in_mpi_ppi, jnt_poss))
arm_jnt_poss = [jnt_poss[i] for i in self.arm_jnt_ik_ids]
return arm_jnt_poss
def load_pose_6dofgraspnet(pose_path):
"""
warning: this function only for grasp pose predicted by
***6dof-graspnet: <https://github.com/rhett-chen/6dof-graspnet> ***
pos is bottom point, ori is rotation matrix
Args:
pose_path: str, grasp pose and score, .npy file
Returns: poses(quaternion, center, approaching vec)(format is customized in get_grasp_points()), score
"""
data = np.load(pose_path, allow_pickle=True).item()
grasps = data['grasp']
scores = data['score']
poses = []
for pose in grasps:
center = np.array(pose[:3, 3])
quat = ut.to_quat(pose[:3, :3])
approching_vec = get_approaching_vec(quat)
poses.append([center, quat, approching_vec])
# center is bottom point, change it to center of two contact points, offset=0.10527 is the distance from bottom
# point to center of two contact points, you can see comments of util.py -> get_grasp_points() for details
offset_along_ori = 0.10527
for center, _, vec in poses:
center += pos_offset_along_approach_vec(vec, offset_along_ori)
return poses, scores
def load_pose_graspnet_baseline(pose_path):
"""
warning: this function only for grasp pose predicted by
*** graspnet-baseline: <https://github.com/rhett-chen/graspnet-baseline> ***
pos is bottom point, ori is rotation matrix
grasp pose format is recorded in https://graspnetapi.readthedocs.io/en/latest/grasp_format.html
Args:
pose_path: str, grasp pose and score, .npy file
Returns: poses(quaternion, center, approaching vec)(format is customized in get_grasp_points()), score
"""
data = np.load(pose_path)
sorted(data, key=lambda x: x[0], reverse=True)
scores = data[:, 0]
rotation_matrix = data[:, 4:13].reshape((-1, 3, 3))
centers = data[:, 13:16]
poses = []
for index, rot in enumerate(rotation_matrix):
approaching_vec = get_approaching_vec(rot, up_vector=[1, 0, 0])
poses.append([centers[index], rot, approaching_vec])
# see why we do the rotation and offset in util.py -> get_grasp_points()
for pose in poses:
rot = pose[1]
te_1 = ut.euler2rot(np.array([0, np.pi / 2, 0]))
te_2 = ut.euler2rot(np.array([np.pi / 2, 0, 0]))
final_rot = np.dot(np.dot(rot, te_2), te_1)
pose[1] = ut.to_quat(final_rot)
offset_along_ori = 0.02
for center, _, vec in poses:
center += pos_offset_along_approach_vec(vec, offset_along_ori)
return poses, scores
def compute_ik(self, pos, ori=None, qinit=None, *args, **kwargs):
"""
Compute the inverse kinematics solution given the
position and orientation of the end effector
(self.cfgs.ARM.ROBOT_EE_FRAME).
Args:
pos (list or np.ndarray): position (shape: :math:`[3,]`).
ori (list or np.ndarray): orientation. It can be euler angles
([roll, pitch, yaw], shape: :math:`[4,]`),
or quaternion ([qx, qy, qz, qw], shape: :math:`[4,]`),
or rotation matrix (shape: :math:`[3, 3]`). If it's None,
the solver will use the current end effector
orientation as the target orientation.
qinit (list or np.ndarray): initial joint positions for numerical
IK (shape: :math:`[6,]`).
Returns:
list: inverse kinematics solution (joint angles)
"""
if ori is not None:
ee_quat = arutil.to_quat(ori)
else:
ee_pos, ee_quat, ee_rot_mat, ee_euler = self.get_ee_pose()
ori_x = ee_quat[0]
ori_y = ee_quat[1]
ori_z = ee_quat[2]
ori_w = ee_quat[3]
if qinit is None:
qinit = self.get_jpos().tolist()
elif isinstance(qinit, np.ndarray):
qinit = qinit.flatten().tolist()
pos_tol = self.cfgs.ARM.IK_POSITION_TOLERANCE
ori_tol = self.cfgs.ARM.IK_ORIENTATION_TOLERANCE
jnt_poss = self._num_ik_solver.get_ik(qinit,
pos[0],
pos[1],
pos[2],
ori_x,
ori_y,
ori_z,
ori_w,
pos_tol,
pos_tol,
pos_tol,
ori_tol,
ori_tol,
ori_tol)
if jnt_poss is None:
return None
return list(jnt_poss)
def load_pose_GPNet(pose_path):
"""
Warning: this function only for grasp pose predicted by ***GPNet***
pos is center of two contact points, ori is quaternion
for GPNet repository, the return hasn't contain scores currently
Args:
pose_path: str, grasp pose txt file path or npy file path
Returns: poses(quaternion, center, approaching vec)(format is customized in get_grasp_points()), score
"""
poses = []
scores = None
data = np.load(pose_path)
for center, quat in data:
approching_vec = get_approaching_vec(quat)
quat = ut.to_quat(quat)
poses.append([center, quat, approching_vec])
return poses, None
def set_ee_pose(self, pos=None, ori=None, wait=True, *args, **kwargs):
"""
Set cartesian space pose of end effector.
Args:
pos (list or np.ndarray): Desired x, y, z positions in the robot's
base frame to move to (shape: :math:`[3,]`).
ori (list or np.ndarray, optional): It can be euler angles
([roll, pitch, yaw], shape: :math:`[4,]`),
or quaternion ([qx, qy, qz, qw], shape: :math:`[4,]`),
or rotation matrix (shape: :math:`[3, 3]`). If it's None,
the solver will use the current end effector
orientation as the target orientation.
wait (bool): wait until the motion completes.
Returns:
bool: Returns True is robot successfully moves to goal pose.
"""
if ori is None and pos is None:
return True
if ori is None:
pose = self.get_ee_pose() # last index is euler angles
quat = pose[1]
else:
quat = arutil.to_quat(ori)
if pos is None:
pose = self.get_ee_pose()
pos = pose[0]
pose = self.moveit_group.get_current_pose()
pose.pose.position.x = pos[0]
pose.pose.position.y = pos[1]
pose.pose.position.z = pos[2]
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
self.moveit_group.set_pose_target(pose)
success = self.moveit_group.go(wait=wait)
return success
def set_ee_pose(self,
pos=None,
ori=None,
wait=True,
ik_first=False,
*args,
**kwargs):
"""
Set cartesian space pose of end effector.
Args:
pos (list or np.ndarray): Desired x, y, z positions in the robot's
base frame to move to (shape: :math:`[3,]`).
ori (list or np.ndarray, optional): It can be euler angles
([roll, pitch, yaw], shape: :math:`[4,]`),
or quaternion ([qx, qy, qz, qw], shape: :math:`[4,]`),
or rotation matrix (shape: :math:`[3, 3]`). If it's None,
the solver will use the current end effector
orientation as the target orientation.
wait (bool): wait until the motion completes.
ik_first (bool, optional): Whether to use the solution computed
by IK, or to use UR built in movel function which moves
linearly in tool space (movel may sometimes fail due to
sinularities). This parameter takes effect only when
self._use_urscript is True. Defaults to False.
Returns:
bool: Returns True is robot successfully moves to goal pose.
"""
if ori is None and pos is None:
return True
success = False
if ori is None:
pose = self.get_ee_pose() # last index is euler angles
quat = pose[1]
else:
quat = arutil.to_quat(ori)
if pos is None:
pose = self.get_ee_pose()
pos = pose[0]
if self._use_urscript:
if ik_first:
jnt_pos = self.compute_ik(pos, quat)
# use movej instead of movel
success = self.set_jpos(jnt_pos, wait=wait)
else:
rotvec = arutil.quat2rotvec(quat)
ee_pos = [
pos[0], pos[1], pos[2], rotvec[0], rotvec[1], rotvec[2]
]
# movel moves linear in tool space, may go through singularity
# orientation in movel is specified as a rotation vector!
# not euler angles!
prog = 'movel(p[%f, %f, %f, %f, %f, %f], a=%f, v=%f, r=%f)' % (
ee_pos[0], ee_pos[1], ee_pos[2], ee_pos[3], ee_pos[4],
ee_pos[5], self._motion_acc, self._motion_vel, 0.0)
self._send_urscript(prog)
if wait:
args_dict = {
'get_func': self.get_ee_pose,
'get_func_derv': self.get_ee_vel,
'timeout': self.cfgs.ARM.TIMEOUT_LIMIT,
'pos_tol': self.cfgs.ARM.MAX_EE_POS_ERROR,
'ori_tol': self.cfgs.ARM.MAX_EE_ORI_ERROR
}
success = wait_to_reach_ee_goal(pos, quat, **args_dict)
else:
pose = self.moveit_group.get_current_pose()
pose.pose.position.x = pos[0]
pose.pose.position.y = pos[1]
pose.pose.position.z = pos[2]
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
self.moveit_group.set_pose_target(pose)
success = self.moveit_group.go(wait=wait)
return success