planner = RRT(fr, is_in_collision)
elif args.rrtc:
print("RRTC: RRT Connect planner is selected!")
planner = RRTConnect(fr, is_in_collision)
elif args.prm:
print("PRM: PRM planner is selected!")
planner = PRM(fr, is_in_collision)
elif args.obprm:
print("OB_PRM: OB_PRM planner is selected!")
planner = OBPRM(fr, is_in_collision)
constraint = ee_upright_constraint
if args.prm or args.obprm:
plan = planner.plan(joints_start, joints_target, constraint, args)
else:
plan = planner.plan(joints_start, joints_target, constraint)
path_quality = get_plan_quality(plan)
print("Path quality: {}".format(path_quality))
collision_boxes_publisher = CollisionBoxesPublisher('collision_boxes')
rate = rospy.Rate(10)
i = 0
while not rospy.is_shutdown():
rate.sleep()
joints = plan[i % len(plan)]
fr.publish_joints(joints)
fr.publish_collision_boxes(joints)
collision_boxes_publisher.publish_boxes(boxes)
i += 1
class FrankaRobot:
joint_limits_low = np.array(
[-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973])
joint_limits_high = np.array(
[2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973])
home_joints = np.array(
[0, -np.pi / 4, 0, -3 * np.pi / 4, 0, np.pi / 2, np.pi / 4])
dh_params = np.array([[0, 0.333, 0, 0], [0, 0, -np.pi / 2, 0],
[0, 0.316, np.pi / 2, 0], [0.0825, 0, np.pi / 2, 0],
[-0.0825, 0.384, -np.pi / 2, 0],
[0, 0, np.pi / 2, 0], [0.088, 0, np.pi / 2, 0],
[0, 0.107, 0, 0], [0, 0.1034, 0, 0]])
num_dof = 7
_dh_alpha_rot = np.array(
[[1, 0, 0, 0], [0, -1, -1, 0], [0, -1, -1, 0], [0, 0, 0, 1]],
dtype=np.float32)
_dh_a_trans = np.array(
[[1, 0, 0, -1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
dtype=np.float32)
_dh_d_trans = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -1], [0, 0, 0, 1]],
dtype=np.float32)
_dh_theta_rot = np.array(
[[-1, -1, 0, 0], [-1, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
dtype=np.float32)
collision_box_shapes = np.array([[0.23, 0.2, 0.1], [0.13, 0.12, 0.1],
[0.12, 0.1, 0.2], [0.15, 0.27, 0.11],
[0.12, 0.1, 0.2], [0.13, 0.12, 0.25],
[0.13, 0.23, 0.15], [0.12, 0.12, 0.4],
[0.12, 0.12, 0.25], [0.13, 0.23, 0.12],
[0.12, 0.12, 0.2], [0.08, 0.22, 0.17]])
_collision_box_links = [1, 1, 1, 1, 1, 3, 4, 5, 5, 5, 7, 7]
_collision_box_poses_raw = np.array(
[[-.04, 0, -0.283, 1, 0, 0, 0], [-0.009, 0, -0.183, 1, 0, 0, 0],
[0, -0.032, -0.082, 0.95141601, 0.30790838, 0, 0],
[-0.008, 0, 0, 1, 0, 0, 0],
[0, .042, .067, 0.95141601, 0.30790838, 0, 0],
[0.00687, 0, -0.139, 1, 0, 0, 0],
[-0.008, 0.004, 0, 0.70710678, -0.70710678, 0, 0],
[0.00422, 0.05367, -0.121, 0.9961947, -0.08715574, 0, 0],
[0.00422, 0.00367, -0.263, 1, 0, 0, 0],
[0.00328, 0.0176, -0.0055, 1, 0, 0, 0],
[-0.0136, 0.0092, 0.0083, 0, 1, 0, 0],
[0.0136, -0.0092, 0.1457, 0.92387953, 0, 0, -0.38268343]])
def __init__(self):
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=10)
self._collision_boxes_pub = CollisionBoxesPublisher(
'franka_collision_boxes')
self._collision_boxes_data = np.zeros(
(len(self.collision_box_shapes), 10))
self._collision_boxes_data[:, -3:] = self.collision_box_shapes
# Precompute things and preallocate np memory for collision checking
self._collision_box_poses = []
for pose in self._collision_box_poses_raw:
T = np.eye(4)
T[:3, 3] = pose[:3]
T[:3, :3] = quaternion.as_rotation_matrix(
quaternion.quaternion(*pose[3:]))
self._collision_box_poses.append(T)
self._collision_box_hdiags = []
self._collision_box_vertices_offset = []
self._vertex_offset_signs = np.array(
list(product([1, -1], [1, -1], [1, -1])))
for sizes in self.collision_box_shapes:
hsizes = sizes / 2
self._collision_box_vertices_offset.append(
self._vertex_offset_signs * hsizes)
self._collision_box_hdiags.append(np.linalg.norm(sizes / 2))
self._collision_box_vertices_offset = np.array(
self._collision_box_vertices_offset)
self._collision_box_hdiags = np.array(self._collision_box_hdiags)
self._collision_proj_axes = np.zeros((3, 15))
self._box_vertices_offset = np.ones([8, 3])
self._box_transform = np.eye(4)
def forward_kinematics(self, joints):
'''
Calculate the position of each joint using the dh_params
Arguments: array of joint positions (rad)
Returns: A numpy array that contains the 4x4 transformation matrices from the base to the position of each joint.
'''
forward_kinematics = np.zeros((len(self.dh_params), 4, 4))
previous_transformation = np.eye(4)
for i in range(len(self.dh_params)):
a, d, alpha, theta = self.dh_params[i]
if i < self.num_dof:
theta = theta + joints[i]
ca, sa = np.cos(alpha), np.sin(alpha)
ct, st = np.cos(theta), np.sin(theta)
self._dh_alpha_rot[1, 1] = ca
self._dh_alpha_rot[1, 2] = -sa
self._dh_alpha_rot[2, 1] = sa
self._dh_alpha_rot[2, 2] = ca
self._dh_a_trans[0, 3] = a
self._dh_d_trans[2, 3] = d
self._dh_theta_rot[0, 0] = ct
self._dh_theta_rot[0, 1] = -st
self._dh_theta_rot[1, 0] = st
self._dh_theta_rot[1, 1] = ct
transform = self._dh_alpha_rot.dot(self._dh_a_trans).dot(
self._dh_d_trans).dot(self._dh_theta_rot)
forward_kinematics[i] = previous_transformation.dot(transform)
previous_transformation = forward_kinematics[i]
return forward_kinematics
def ee(self, joints):
'''
Arguments: array of joint positions (rad)
Returns: A numpy array that contains the [x, y, z, roll, pitch, yaw] location of the end-effector.
'''
fk = self.forward_kinematics(joints)
ee_frame = fk[-1, :, :]
x, y, z = ee_frame[:-1, 3]
roll = np.arctan2(ee_frame[2, 1], ee_frame[2, 2])
pitch = np.arcsin(-ee_frame[2, 0])
yaw = np.arctan2(ee_frame[1, 0], ee_frame[0, 0])
return np.array([x, y, z, roll, pitch, yaw])
def jacobian(self, joints):
'''
Calculate the jacobians analytically using your forward kinematics
Arguments: array of joint positions (rad)
Returns: A numpy array that contains the 6 x num_dof end-effector jacobian.
'''
jacobian = np.zeros((6, self.num_dof))
fk = self.forward_kinematics(joints)
ee_pos = fk[-1, :3, 3]
for i in range(self.num_dof):
joint_pos = fk[i, :3, 3]
joint_axis = fk[i, :3, 2]
jacobian[:3, i] = np.cross(joint_axis, (ee_pos - joint_pos)).T
jacobian[3:, i] = joint_axis.T
return jacobian
def inverse_kinematics(self, desired_ee_pos, current_joints):
'''
Arguments: desired_ee_pos which is a np array of [x, y, z, r, p, y] which represent the desired end-effector position of the robot
current_joints which represents the current location of the robot
Returns: A numpy array that contains the joints required in order to achieve the desired end-effector position.
'''
joints = current_joints.copy()
current_ee_pos = self.ee(joints)
ee_error = desired_ee_pos - current_ee_pos
alpha = 0.1
while np.linalg.norm(ee_error) > 1e-3:
jacob = self.jacobian(joints)
joints += alpha * jacob.T.dot(ee_error.T)
current_ee_pos = self.ee(joints)
ee_error = desired_ee_pos - current_ee_pos
return joints
def check_box_collision(self, joints, box):
'''
Arguments: joints represents the current location of the robot
box contains the position of the center of the box [x, y, z, r, p, y] and the length, width, and height [l, w, h]
Returns: A boolean where True means the box is in collision with the arm and false means that there are no collisions.
'''
box_pos, box_rpy, box_hsizes = box[:3], box[3:6], box[6:] / 2
box_q = quaternion.from_euler_angles(box_rpy)
box_axes = quaternion.as_rotation_matrix(box_q)
self._box_vertices_offset[:, :] = self._vertex_offset_signs * box_hsizes
box_vertices = (box_axes.dot(self._box_vertices_offset.T) +
np.expand_dims(box_pos, 1)).T
box_hdiag = np.linalg.norm(box_hsizes)
min_col_dists = box_hdiag + self._collision_box_hdiags
franka_box_poses = self.get_collision_boxes_poses(joints)
for i, franka_box_pose in enumerate(franka_box_poses):
fbox_pos = franka_box_pose[:3, 3]
fbox_axes = franka_box_pose[:3, :3]
# coarse collision check
if np.linalg.norm(fbox_pos - box_pos) > min_col_dists[i]:
continue
fbox_vertex_offsets = self._collision_box_vertices_offset[i]
fbox_vertices = fbox_vertex_offsets.dot(fbox_axes.T) + fbox_pos
# construct axes
cross_product_pairs = np.array(
list(product(box_axes.T, fbox_axes.T)))
cross_axes = np.cross(cross_product_pairs[:, 0],
cross_product_pairs[:, 1]).T
self._collision_proj_axes[:, :3] = box_axes
self._collision_proj_axes[:, 3:6] = fbox_axes
self._collision_proj_axes[:, 6:] = cross_axes
# projection
box_projs = box_vertices.dot(self._collision_proj_axes)
fbox_projs = fbox_vertices.dot(self._collision_proj_axes)
min_box_projs, max_box_projs = box_projs.min(
axis=0), box_projs.max(axis=0)
min_fbox_projs, max_fbox_projs = fbox_projs.min(
axis=0), fbox_projs.max(axis=0)
# check if no separating planes exist
if np.all([
min_box_projs <= max_fbox_projs,
max_box_projs >= min_fbox_projs
]):
return True
return False
def publish_joints(self, joints):
joint_state = JointState()
joint_state.name = [
'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
'panda_joint5', 'panda_joint6', 'panda_joint7',
'panda_finger_joint1', 'panda_finger_joint2'
]
joint_state.header.stamp = rospy.Time.now()
if len(joints) == 7:
joints = np.concatenate([joints, [0, 0]])
joint_state.position = joints
self._joint_state_pub.publish(joint_state)
def get_collision_boxes_poses(self, joints):
fk = self.forward_kinematics(joints)
box_poses_world = []
for i, link in enumerate(self._collision_box_links):
link_transform = fk[link - 1]
box_pose_world = link_transform.dot(self._collision_box_poses[i])
box_poses_world.append(box_pose_world)
return box_poses_world
def publish_collision_boxes(self, joints):
box_poses_world = self.get_collision_boxes_poses(joints)
for i, pose in enumerate(box_poses_world):
self._collision_boxes_data[i, :3] = pose[:3, 3]
q = quaternion.from_rotation_matrix(pose[:3, :3])
for j, k in enumerate('wxyz'):
self._collision_boxes_data[i, 3 + j] = getattr(q, k)
self._collision_boxes_pub.publish_boxes(self._collision_boxes_data)