def translate_robot(robot_model, joint_list, trans):
assert len(trans) == 2
av_current = get_robot_config(robot_model, joint_list, with_base=True)
av_next = copy.copy(av_current)
av_next[-3:-1] += np.array(trans)
set_robot_config(robot_model, joint_list, av_next, with_base=True)
return av_current, av_next
def update_color(self): # for debugging
"""Update the color of links under collision
This method checks the collision between the collision
spheres and registered sdf. If collision spheres are
found to be under collision, the color of the spheres
will be changed to `color_collision_sphere`.
Returns
-------
dists : numpy.ndarray(n_sphere,)
array of the signed distances for each sphere against sdf.
"""
joint_list = [j for j in self.robot_model.joint_list]
angle_vector = get_robot_config(self.robot_model,
joint_list,
with_base=True)
dists, _ = self.compute_batch_sd_vals(joint_list,
np.array([angle_vector]),
with_base=True,
with_jacobian=False)
idxes_collide = np.where(dists < 0)[0]
for idx in range(self.n_feature):
sphere = self.coll_sphere_list[idx]
n_facet = len(sphere._visual_mesh.visual.face_colors)
color = self.color_collision_sphere if idx in idxes_collide \
else self.color_normal_sphere
sphere._visual_mesh.visual.face_colors = np.array([color] *
n_facet)
return dists
def test_set_and_get_robot_config(self):
robot_model = self.robot_model
joint_list = self.joint_list
av = self.av
av_with_base = self.av_with_base
with self.assertRaises(AssertionError):
set_robot_config(robot_model, joint_list, av, with_base=True)
set_robot_config(robot_model, joint_list, av, with_base=False)
with self.assertRaises(AssertionError):
set_robot_config(robot_model, joint_list,
av_with_base, with_base=False)
set_robot_config(robot_model, joint_list, av_with_base, with_base=True)
testing.assert_equal(
av,
get_robot_config(robot_model, joint_list, with_base=False)
)
testing.assert_equal(
av_with_base,
get_robot_config(robot_model, joint_list, with_base=True)
)
def collision_check(self):
"""Check collision between links and collision spheres.
Returns
-------
is_collision : bool
`True` if a collision occurred between any pair of links and
collision spheres and `False` otherwise.
"""
joint_list = [j for j in self.robot_model.joint_list]
angle_vector = get_robot_config(self.robot_model,
joint_list,
with_base=True)
dists, _ = self.compute_batch_sd_vals(joint_list,
np.array([angle_vector]),
with_base=True,
with_jacobian=False)
idxes_collide = np.where(dists < 0)[0]
return len(idxes_collide) > 0
if with_base:
av_start = np.hstack([av_start, [0, 0, 0]])
# solve inverse kinematics to obtain av_goal
coef = 3.1415 / 180.0
joint_angles = [coef * e for e in [-60, 74, -70, -120, -20, -30, 180]]
set_robot_config(robot_model, joint_list, joint_angles)
rarm_end_coords = skrobot.coordinates.CascadedCoords(
parent=robot_model.r_gripper_tool_frame, name='rarm_end_coords')
robot_model.inverse_kinematics(target_coords=skrobot.coordinates.Coordinates(
[0.8, -0.6, 0.8], [0, 0, 0]),
link_list=link_list,
move_target=robot_model.rarm_end_coords,
rotation_axis=True)
av_goal = get_robot_config(robot_model, joint_list, with_base=with_base)
# collision checker setup
sscc = SweptSphereSdfCollisionChecker(lambda X: box.sdf(X), robot_model)
for link in coll_link_list:
sscc.add_collision_link(link)
# motion planning
ts = time.time()
av_seq = sqp_plan_trajectory(sscc,
av_start,
av_goal,
joint_list,
10,
safety_margin=1e-2,
with_base=with_base)
if __name__ == '__main__':
robot_model = skrobot.models.PR2()
table = Box(extents=[0.6, 0.1, 0.1], with_sdf=True)
table.translate([0.5, -0.4, 1.4])
#fksolver = tinyfk.RobotModel(robot_model.urdf_path)
coll_link_list = rarm_coll_link_list(robot_model)
joint_list = rarm_joint_list(robot_model)
joint_names = [j.name for j in joint_list]
joint_ids = robot_model.fksolver.get_joint_ids(joint_names)
collink_ids = robot_model.fksolver.get_link_ids(
[l.name for l in coll_link_list])
ef_id = robot_model.fksolver.get_link_ids(["r_gripper_tool_frame"])[0]
q = get_robot_config(robot_model, joint_list, with_base=False)
qd = q * 0.0
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(641, 480))
viewer.add(robot_model)
viewer.add(table)
viewer.show()
x_goal = np.array([0.4, -0.8, 0.7])
rmp_target = TargetRMP(x_goal)
rmp_collision = CollisionRMP(table.sdf)
rmp_rc = RobotCollisionRMP(table.sdf)
time.sleep(1.0)
dt = 1e-2
if with_move_base:
# TODO
print(
"NOTE that, to use this, one needs to remove point cloud on robot")
ri.go_pos_unsafe(x=0.1, wait=True)
topic_name = "/voxblox_node/esdf_map_out"
esdf = EsdfMapClientInterface(0.05, 16, 100.0)
def callback(msg):
print("rec")
esdf.update(msg)
rospy.Subscriber(topic_name, Layer, callback)
time.sleep(18.0)
av_start = get_robot_config(robot_model, joint_list, with_base=False)
# solve inverse kinematics to obtain av_goal
joint_angles = np.deg2rad([-60, 74, -70, -120, -20, -30, 180])
set_robot_config(robot_model, joint_list, joint_angles)
target_coords = skrobot.coordinates.Coordinates([0.9, -0.4, 1.0], [0, 0, 0])
rarm_end_coords = skrobot.coordinates.CascadedCoords(
parent=robot_model.r_gripper_tool_frame, name='rarm_end_coords')
robot_model.inverse_kinematics(target_coords=target_coords,
link_list=link_list,
move_target=robot_model.rarm_end_coords,
rotation_axis=False)
av_goal = get_robot_config(robot_model, joint_list, with_base=False)
if nodebug:
sscc = TinyfkSweptSphereSdfCollisionChecker(lambda X: fridge.sdf(X),
robot_model)
for link in rarm_coll_link_list(robot_model):
sscc.add_collision_link(link)
joint_list = rarm_joint_list(robot_model)
with_base = True
# constraint manager
n_wp = 14
fksolver = sscc.fksolver # TODO temporary
cm = ConstraintManager(n_wp, [j.name for j in joint_list], fksolver, with_base)
update_fksolver(fksolver, robot_model)
av_start = get_robot_config(robot_model, joint_list, with_base=with_base)
cm.add_eq_configuration(0, av_start)
cm.add_pose_constraint(10, "r_gripper_tool_frame",
[1.7, 2.2, 1.0, 0.0, 0.0, 0.0])
cm.add_multi_pose_constraint(11,
["r_gripper_tool_frame", "l_gripper_tool_frame"],
[[1.6, 2.1, 1.0, 0.0, 0.0, 0.6], [1.7, 2.2, 1.0]])
cm.add_multi_pose_constraint(12,
["r_gripper_tool_frame", "l_gripper_tool_frame"],
[[1.5, 2.0, 1.0, 0.0, 0.0, 0.6], [1.7, 2.2, 1.0]])
cm.add_multi_pose_constraint(13,
["r_gripper_tool_frame", "l_gripper_tool_frame"],
[[1.4, 1.9, 1.0, 0.0, 0.0, 0.6], [1.7, 2.2, 1.0]])
av_current = get_robot_config(robot_model, joint_list, with_base=with_base)
av_seq_init = cm.gen_initial_trajectory(av_current)