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 test_compute_batch_sd_vals(self):
robot_model = self.robot_model
joint_list = self.joint_list
av = np.array([0.4, 0.6] + [-0.7] * 5)
set_robot_config(robot_model, joint_list, av)
def func(av):
return self.coll_checker.compute_batch_sd_vals(joint_list, [av],
with_jacobian=True)
jacobian_test_util(func, av)
def test_update_color(self):
robot_model = self.robot_model
av = np.array([0.4, 0.6] + [-0.7] * 5)
av_with_base = np.hstack((av, [0.1, 0.0, 0.3]))
joint_list = self.joint_list
set_robot_config(robot_model, joint_list, av_with_base, with_base=True)
dists_ground_truth = np.array([
0.15608007, 0.06519901, -0.01225516, -0.04206324, -0.01748433,
-0.01476751, -0.01205069, -0.00933387, -0.00681298, -0.03081892,
-0.02670986, 0.02851278, 0.01134911
])
dists = self.coll_checker.update_color()
testing.assert_almost_equal(dists, dists_ground_truth)
idxes_colliding = np.where(dists < 0)[0]
color_collide = self.coll_checker.color_collision_sphere
for idx in idxes_colliding:
sphere = self.coll_checker.coll_sphere_list[idx]
color_actual = sphere._visual_mesh.visual.face_colors[0]
testing.assert_equal(color_actual, color_collide)
def test_coll_batch_forward_kinematics(self):
robot_model = self.robot_model
av = np.array([0.4, 0.6] + [-0.7] * 5)
av_with_base = np.hstack((av, [0.1, 0.0, 0.3]))
joint_list = self.joint_list
set_robot_config(robot_model, joint_list, av_with_base, with_base=True)
n_wp = 1
n_feature = self.coll_checker.n_feature
def collision_fk(av, with_base):
# this function wraps _coll_batch_forward_kinematics so that it's
# output will be scipy-style (f, jac)
n_dof = len(av)
f_tmp, jac_tmp = self.coll_checker._coll_batch_forward_kinematics(
joint_list, [av], with_base=with_base, with_jacobian=True)
f = f_tmp.flatten()
jac = jac_tmp.reshape((n_wp * n_feature * 3, n_dof))
return f, jac
jacobian_test_util(lambda av: collision_fk(av, False), av)
jacobian_test_util(lambda av: collision_fk(av, True), av_with_base)
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)
)
coll_link_list = [
robot_model.r_upper_arm_link, robot_model.r_forearm_link,
robot_model.r_gripper_palm_link, robot_model.r_gripper_r_finger_link,
robot_model.r_gripper_l_finger_link
]
# obtain av_start (please try both with_base=True, FalseA)
with_base = True
av_start = np.array([0.564, 0.35, -0.74, -0.7, -0.7, -0.17, -0.63])
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)
from skrobot.models import PR2
from skrobot.model import Box
from skrobot.planner.utils import get_robot_config
from skrobot.planner.utils import set_robot_config
table = Box(extents=[0.1, 0.1, 3.0], with_sdf=True)
table.translate([0.7, -0.45, 1.2])
mech = kinatt.create_pr2_mechanism()
with_base = True
pose_const = PoseConstraint(mech,
"r_gripper_tool_frame", [0.2, -0.8, 0.6],
with_base=with_base)
objfun = ObjectiveFunction.from_constraint(pose_const)
colavoid = CollisionConstraint(mech, table.sdf, with_base=with_base)
attractor = Attractor(objfun, colavoid)
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(641, 480))
robot = PR2()
joint_list = [robot.__dict__[name] for name in mech.joint_names]
viewer.add(robot)
viewer.add(table)
viewer.show()
joint_angles = np.zeros(17)
for i in range(200):
joint_angles = attractor.propagate(joint_angles, radius=0.02)
set_robot_config(robot, joint_list, joint_angles, with_base=with_base)
viewer.redraw()
time.sleep(0.1)
dt = 1e-2
for i in range(10000):
print(i)
P, J = robot_model.fksolver.solve_forward_kinematics(
[q], [ef_id],
joint_ids,
with_rot=False,
with_base=False,
with_jacobian=True,
use_cache=False)
x = P[0]
x_dot = J.dot(qd)
rmp_pullback_target = pull_back(rmp_target, J)
rmp_pullback_collision = pull_back(rmp_collision, J)
ts = time.time()
f1, A1 = rmp_rc.resolve(q, qd)
f2 = rmp_pullback_target.f(x, x_dot)
A2 = rmp_pullback_target.A(x, x_dot)
f_whole = np.linalg.pinv(A1 + A2).dot(A1.dot(f1) + A2.dot(f2))
#print(time.time() - ts)
rmp_sum = sum_rmp(rmp_pullback_target, rmp_pullback_collision)
qdd_sum = f_whole
qd = qd + qdd_sum * dt
q = get_robot_config(robot_model, joint_list, with_base=False)
set_robot_config(robot_model, joint_list, q + qd * dt, with_base=False)
viewer.redraw()
time.sleep(0.01)
