def __init__(self, *args, **kwargs):
super(Fetch, self).__init__(*args, **kwargs)
self.rarm_end_coords = CascadedCoords(parent=self.gripper_link,
name='rarm_end_coords')
self.rarm_end_coords.translate([0, 0, 0])
self.rarm_end_coords.rotate(0, axis='z')
self.end_coords = [self.rarm_end_coords]
class Fetch(RobotModelFromURDF):
"""Fetch Robot Model.
http://docs.fetchrobotics.com/robot_hardware.html
"""
def __init__(self, *args, **kwargs):
super(Fetch, self).__init__(*args, **kwargs)
self.rarm_end_coords = CascadedCoords(parent=self.gripper_link,
name='rarm_end_coords')
self.rarm_end_coords.translate([0, 0, 0])
self.rarm_end_coords.rotate(0, axis='z')
self.end_coords = [self.rarm_end_coords]
@cached_property
def default_urdf_path(self):
return fetch_urdfpath()
def reset_pose(self):
self.torso_lift_joint.joint_angle(0)
self.shoulder_pan_joint.joint_angle(np.deg2rad(75.6304))
self.shoulder_lift_joint.joint_angle(np.deg2rad(80.2141))
self.upperarm_roll_joint.joint_angle(np.deg2rad(-11.4592))
self.elbow_flex_joint.joint_angle(np.deg2rad(98.5487))
self.forearm_roll_joint.joint_angle(0.0)
self.wrist_flex_joint.joint_angle(np.deg2rad(95.111))
self.wrist_roll_joint.joint_angle(0.0)
self.head_pan_joint.joint_angle(0.0)
self.head_tilt_joint.joint_angle(0.0)
return self.angle_vector()
def reset_manip_pose(self):
self.torso_lift_joint.joint_angle(0)
self.shoulder_pan_joint.joint_angle(0)
self.shoulder_lift_joint.joint_angle(0)
self.upperarm_roll_joint.joint_angle(0)
self.elbow_flex_joint.joint_angle(np.pi / 2.0)
self.forearm_roll_joint.joint_angle(0)
self.wrist_flex_joint.joint_angle(- np.pi / 2.0)
self.wrist_roll_joint.joint_angle(0)
self.head_pan_joint.joint_angle(0)
self.head_tilt_joint.joint_angle(0)
return self.angle_vector()
@cached_property
def rarm(self):
rarm_links = [self.shoulder_pan_link,
self.shoulder_lift_link,
self.upperarm_roll_link,
self.elbow_flex_link,
self.forearm_roll_link,
self.wrist_flex_link,
self.wrist_roll_link]
rarm_joints = []
for link in rarm_links:
rarm_joints.append(link.joint)
r = RobotModel(link_list=rarm_links,
joint_list=rarm_joints)
r.end_coords = self.rarm_end_coords
return r
class Kuka(RobotModelFromURDF):
"""Kuka Robot Model."""
def __init__(self, *args, **kwargs):
super(Kuka, self).__init__(*args, **kwargs)
self.rarm_end_coords = CascadedCoords(
parent=self.lbr_iiwa_with_wsg50__lbr_iiwa_link_7,
name='rarm_end_coords')
self.rarm_end_coords.translate(
np.array([0.0, 0.030, 0.250], dtype=np.float32))
self.rarm_end_coords.rotate(-np.pi / 2.0, axis='y')
self.rarm_end_coords.rotate(-np.pi / 2.0, axis='x')
self.end_coords = [self.rarm_end_coords]
@cached_property
def default_urdf_path(self):
return kuka_urdfpath()
def reset_manip_pose(self):
return self.angle_vector([
0,
np.deg2rad(10), 0,
np.deg2rad(-90), 0,
np.deg2rad(90), 0, 0, 0, 0, 0, 0
])
@cached_property
def rarm(self):
rarm_links = [
self.lbr_iiwa_with_wsg50__lbr_iiwa_link_1,
self.lbr_iiwa_with_wsg50__lbr_iiwa_link_2,
self.lbr_iiwa_with_wsg50__lbr_iiwa_link_3,
self.lbr_iiwa_with_wsg50__lbr_iiwa_link_4,
self.lbr_iiwa_with_wsg50__lbr_iiwa_link_5,
self.lbr_iiwa_with_wsg50__lbr_iiwa_link_6,
self.lbr_iiwa_with_wsg50__lbr_iiwa_link_7
]
rarm_joints = []
for link in rarm_links:
if hasattr(link, 'joint'):
rarm_joints.append(link.joint)
r = RobotModel(link_list=rarm_links, joint_list=rarm_joints)
r.end_coords = self.rarm_end_coords
return r
def close_hand(self, av=None):
if av is None:
av = self.angle_vector()
av[-2] = 0
av[-4] = 0
return self.angle_vector(av)
def open_hand(self, default_angle=np.deg2rad(10), av=None):
if av is None:
av = self.angle_vector()
av[-2] = default_angle
av[-4] = -default_angle
return self.angle_vector(av)
def __init__(self, origin, coords=None, use_abs=False):
if coords is None:
coords = CascadedCoords()
self.coords = coords
self.sdf_to_obj_transform = CascadedCoords(
pos=origin)
self._origin = np.array(origin)
self.use_abs = use_abs
def __init__(self, *args, **kwargs):
super(Kuka, self).__init__(*args, **kwargs)
self.rarm_end_coords = CascadedCoords(
parent=self.lbr_iiwa_with_wsg50__lbr_iiwa_link_7,
name='rarm_end_coords')
self.rarm_end_coords.translate(
np.array([0.0, 0.030, 0.250], dtype=np.float32))
self.rarm_end_coords.rotate(-np.pi / 2.0, axis='y')
self.rarm_end_coords.rotate(-np.pi / 2.0, axis='x')
self.end_coords = [self.rarm_end_coords]
def __init__(self, *args, **kwargs):
super(Spot, self).__init__(*args, **kwargs)
self.rfront_end_coords = CascadedCoords(
parent=self.front_right_toe_link, name='rfont_end_coords')
self.lfront_end_coords = CascadedCoords(
parent=self.front_left_toe_link, name='lfont_end_coords')
self.rrear_end_coords = CascadedCoords(parent=self.rear_right_toe_link,
name='rrear_end_coords')
self.lrear_end_coords = CascadedCoords(parent=self.rear_left_toe_link,
name='lrear_end_coords')
def __init__(self, *args, **kwargs):
super(PR2, self).__init__(*args, **kwargs)
self.rarm_end_coords = CascadedCoords(parent=self.r_gripper_tool_frame,
name='rarm_end_coords')
self.larm_end_coords = CascadedCoords(parent=self.l_gripper_tool_frame,
name='larm_end_coords')
self.head_end_coords = CascadedCoords(pos=[0.08, 0.0, 0.13],
parent=self.head_tilt_link,
name='head_end_coords').rotate(
np.pi / 2.0, 'y')
self.torso_end_coords = CascadedCoords(parent=self.torso_lift_link,
name='head_end_coords')
# limbs
self.torso = [self.torso_lift_link]
self.torso_root_link = self.torso_lift_link
self.larm_root_link = self.l_shoulder_pan_link
self.rarm_root_link = self.r_shoulder_pan_link
self.head_root_link = self.head_pan_link
# custom min_angle and max_angle for joints
joint_list = [
self.torso_lift_joint, self.l_shoulder_pan_joint,
self.l_shoulder_lift_joint, self.l_upper_arm_roll_joint,
self.l_elbow_flex_joint, self.l_forearm_roll_joint,
self.l_wrist_flex_joint, self.l_wrist_roll_joint,
self.r_shoulder_pan_joint, self.r_shoulder_lift_joint,
self.r_upper_arm_roll_joint, self.r_elbow_flex_joint,
self.r_forearm_roll_joint, self.r_wrist_flex_joint,
self.r_wrist_roll_joint, self.head_pan_joint, self.head_tilt_joint
]
for j, min_angle, max_angle in zip(
joint_list,
(0.0115, np.deg2rad(-32.3493), np.deg2rad(-20.2598),
np.deg2rad(-37.2423), np.deg2rad(-121.542), -float('inf'),
np.deg2rad(-114.592), -float('inf'), np.deg2rad(-122.349),
np.deg2rad(-20.2598), np.deg2rad(-214.859), np.deg2rad(-121.542),
-float('inf'), np.deg2rad(-114.592), -float('inf'),
np.deg2rad(-163.694), np.deg2rad(-21.2682)),
(0.325, np.deg2rad(122.349), np.deg2rad(74.2725),
np.deg2rad(214.859), np.deg2rad(-8.59437), float('inf'),
np.deg2rad(-5.72958), float('inf'), np.deg2rad(32.3493),
np.deg2rad(74.2725), np.deg2rad(37.2423), np.deg2rad(-8.59437),
float('inf'), np.deg2rad(-5.72958), float('inf'),
np.deg2rad(163.694), np.deg2rad(74.2702))):
j.min_angle = min_angle
j.max_angle = max_angle
def _forward_kinematics(robot_model, joint_list, move_target, with_rot,
with_base, with_jacobian):
link_list = [joint.child_link for joint in joint_list]
ef_pos_wrt_world = move_target.worldpos()
ef_quat_wrt_world = move_target.worldcoords().quaternion
world_coordinate = CascadedCoords()
def quaternion_kinematic_matrix(q):
# dq/dt = 0.5 * mat * omega
q1, q2, q3, q4 = q
mat = np.array([[-q2, -q3, -q4], [q1, q4, -q3], [-q4, q1, q2],
[q3, -q2, q1]])
return mat * 0.5
def compute_jacobian_wrt_world():
J_joint = robot_model.calc_jacobian_from_link_list(
[move_target],
link_list,
transform_coords=world_coordinate,
rotation_axis=with_rot)
if with_rot:
kine_mat = quaternion_kinematic_matrix(ef_quat_wrt_world)
J_joint_rot_geometric = J_joint[3:, :] # "geometric" jacobian
J_joint_quat = kine_mat.dot(J_joint_rot_geometric)
J_joint = np.vstack((J_joint[:3, :], J_joint_quat))
if with_base: # cat base jacobian if base is considered
# please follow computation carefully
base_pos_wrt_world = robot_model.worldpos()
ef_pos_wrt_world = move_target.worldpos()
ef_pos_wrt_base = ef_pos_wrt_world - base_pos_wrt_world
x, y = ef_pos_wrt_base[0], ef_pos_wrt_base[1]
J_base_pos = np.array([[1, 0, -y], [0, 1, x], [0, 0, 0]])
if with_rot:
J_base_quat_xy = np.zeros((4, 2))
rot_axis = np.array([0, 0, 1.0])
J_base_quat_theta = kine_mat.dot(rot_axis).reshape(4, 1)
J_base_quat = np.hstack((J_base_quat_xy, J_base_quat_theta))
J_base = np.vstack((J_base_pos, J_base_quat))
else:
J_base = J_base_pos
J_whole = np.hstack((J_joint, J_base))
else:
J_whole = J_joint
return J_whole
pose = np.hstack((ef_pos_wrt_world, ef_quat_wrt_world)) if with_rot \
else ef_pos_wrt_world
if with_jacobian:
J = compute_jacobian_wrt_world()
return pose, J
else:
return pose, None
def test_is_relevant(self):
fetch = self.fetch
self.assertTrue(fetch._is_relevant(
fetch.shoulder_pan_joint, fetch.wrist_roll_link))
self.assertFalse(fetch._is_relevant(
fetch.shoulder_pan_joint, fetch.base_link))
self.assertTrue(fetch._is_relevant(
fetch.shoulder_pan_joint, fetch.rarm_end_coords))
co = make_coords()
with self.assertRaises(AssertionError):
fetch._is_relevant(fetch.shoulder_pan_joint, co)
# if it's not connceted to the robot,
casco = CascadedCoords()
with self.assertRaises(AssertionError):
fetch._is_relevant(fetch.shoulder_pan_joint, casco)
# but, if casco connects to the robot,
fetch.rarm_end_coords.assoc(casco)
self.assertTrue(fetch._is_relevant(
fetch.shoulder_pan_joint, casco))
def add_collision_link(self, coll_link):
"""Add link for which collision with sdf is checked
The given `coll_link` will be approximated by swept-spheres
and these spheres will be added to collision sphere's list.
Parameters
----------
coll_link : list[skrobot.model.Link]
link for which collision with sdf is checked
"""
if coll_link.name in self.coll_link_name_list:
return
self.coll_link_name_list.append(coll_link)
col_mesh = coll_link.collision_mesh
assert type(col_mesh) == trimesh.base.Trimesh
centers, R = compute_swept_sphere(col_mesh)
sphere_list = []
coords_list = []
for center in centers:
link_pos = coll_link.copy_worldcoords()
coll_coords = CascadedCoords(pos=link_pos.worldpos(),
rot=link_pos.worldrot())
coll_coords.translate(center)
coll_link.assoc(coll_coords)
coords_list.append(coll_coords)
# add sphere
sp = Sphere(radius=R,
pos=coll_coords.worldpos(),
color=self.color_normal_sphere)
coll_coords.assoc(sp)
sphere_list.append(sp)
self.coll_sphere_list.extend(sphere_list)
self.coll_coords_list.extend(coords_list)
self.coll_radius_list.extend([R] * len(sphere_list))
self.n_feature = len(self.coll_coords_list)
def __init__(self, sdf_data, origin, resolution,
fill_value=np.inf, coords=None, use_abs=False):
super(GridSDF, self).__init__(origin, coords=coords, use_abs=use_abs)
# optionally use only the absolute values
# (useful for non-closed meshes in 3D)
self._data = np.abs(sdf_data) if use_abs else sdf_data
self._dims = np.array(self._data.shape)
self._resolution = resolution
self._surface_threshold = resolution * np.sqrt(2) / 2.0
# create regular grid interpolator
xlin, ylin, zlin = [
np.array(range(d)) * resolution for d in self._data.shape]
self.itp = RegularGridInterpolator(
(xlin, ylin, zlin),
self._data,
bounds_error=False,
fill_value=fill_value)
spts, _ = self._surface_points()
self.sdf_to_obj_transform = CascadedCoords(
pos=origin)
robot_model.r_wrist_roll_link, robot_model.base_link,
robot_model.r_upper_arm_link, mylink
]
joint_angles = [0.564, 0.35, -0.74, -0.7, -0.7, -0.17, -0.63]
for j, a in zip(joint_list, joint_angles):
j.joint_angle(a)
x, y, theta = 0.3, 0.5, 0.7
co = Coordinates(pos=[x, y, 0.0], rot=rpy_matrix(theta, 0.0, 0.0))
robot_model.newcoords(co)
angle_vector = joint_angles + [x, y, theta]
# compute pose of links
world_coordinate = CascadedCoords()
def extract_pose(co):
return np.hstack((co.worldpos(), co.worldcoords().rpy_angle()[0]))
pose_list = [extract_pose(co).tolist() for co in link_list]
# compute jacobian of links
world_coordinate = CascadedCoords()
J = [
robot_model.calc_jacobian_from_link_list(
link, [j.child_link for j in joint_list],
transform_coords=world_coordinate,
rotation_axis=True).tolist() for link in link_list