def test_err_obj_in_manip():
import numdifftools
env = cans_world_env()
obj17 = env.GetKinBody('object17')
tran = obj17.GetTransform()
tran[0,3] = .49268
tran[1,3] = -.51415
obj17.SetTransform(tran)
obj_K = 6
obj_traj_val = obj_pose_from_transform(tran).reshape((obj_K,1))
robot = PR2('pr2')
robot.tuck_arms(env)
robot._set_active_dofs(env, ['rightarm'])
pose = robot.get_pose(env)
obj = Obj('object17')
gp_val = np.array([0,0,.125]).reshape((3,1))
gp = HLParam("gp", (3, 1), is_var=False, value=gp_val)
# traj_val = np.array([[-1.4352011 ],\
# [ 0.63522797],\
# [-1.19804084],\
# [-1.55807855],\
# [-0.9962505 ],\
# [-0.75279673],\
# [-1.13988334]])
traj_val = np.array([[-1.07265580e+00], [5.17875957e-01], [-1.30000000e+00], \
[-1.08244363e+00], [-1.04143184e+00], [-9.06938766e-02], [-5.38403045e-01]])
dummy_action = DummyInManipAction()
traj = Traj(dummy_action, "traj", (7, 1), is_var=True, value=traj_val)
obj_traj = Traj(dummy_action, "obj_traj", (obj_K, 1), is_var=False, value=obj_traj_val)
fluent = ObjInManip(env, dummy_action, robot, 0, obj, gp, traj, obj_traj)
# val, jac = fluent.pos_error(obj_traj_val.flatten())
# jac_fn = numdifftools.Jacobian(lambda x: fluent.pos_error(x)[0])
# num_jac = jac_fn(obj_traj_val.flatten())[0]
# print "analytic pos jac:\t{}\tnumerical pos jac:\t{}\tthe same?:\t{}".format(jac, num_jac, np.allclose(jac, num_jac))
val, jac = fluent.rot_error(obj_traj_val.flatten())
jac_fn = numdifftools.Jacobian(lambda x: fluent.rot_error(x)[0])
num_jac = jac_fn(obj_traj_val.flatten())[0]
print "analytic rot jac:\t{}\tnumerical rot jac:\t{}\tthe same?:\t{}".format(jac, num_jac, np.allclose(jac, num_jac))
def get_pose(self, env):
obj = self.get_env_body(env)
obj_transform = obj.GetTransform()
pose = obj_pose_from_transform(obj_transform)
return pose
def test_move_pr2_with_obj_wrt_obj():
env = cans_world_env()
obj17 = env.GetKinBody('object17')
tran = obj17.GetTransform()
tran[0,3] = .49268
tran[1,3] = -.51415
obj17.SetTransform(tran)
hl_plan = TestDomain(env)
move_env = env.CloneSelf(1) # clones objects in the environment
move_pr2 = move_env.GetKinBody('pr2')
move_pr2.SetActiveDOFs(move_pr2.GetManipulator('rightarm').GetArmIndices())
move_pr2.SetDOFValues([0.3],
[move_pr2.GetJoint("torso_lift_joint").GetDOFIndex()])
robot = PR2('pr2')
robot.tuck_arms(env)
robot._set_active_dofs(env, ['rightarm'])
pose = robot.get_pose(env)
hl_plan = HLPlan(env)
obj = Obj('object17')
K = 7
# obj_pose = np.array([[0.],[0.],[0.]])
# obj.set_pose(env, obj_pose)
# start = HLParam("pos", (self.K, 1), is_var=False, value=robot.get_pose(env))
start_pose = np.array([[-1.4352011 ],\
[ 0.63522797],\
[-1.19804084],\
[-1.55807855],\
[-0.9962505 ],\
[-0.75279673],\
[-1.13988334]])
end_pose = np.array([[-1.28868338],\
[ 0.53921863],\
[-1.41293145],\
[-1.67185359],\
[-0.74117023],\
[-0.73603889],\
[-1.29004773]])
robot.set_pose(env, start_pose)
# traj_val = lininterp(start_pose, end_pose, 10)
traj_val = lininterp(start_pose, end_pose, 2)
# getting grasp pose
robot_body = robot.get_env_body(env)
rarm = robot_body.GetManipulator('rightarm')
# import ipdb; ipdb.set_trace()
W_T_EE = rarm.GetEndEffectorTransform()
EE_T_W = np.linalg.inv(W_T_EE)
W_T_O = obj17.GetTransform()
EE_T_O = np.dot(EE_T_W, W_T_O)
EE_T_O_pose = poseFromMatrix(EE_T_O)
# gp = HLParam("gp", (7, 1), is_var=False, value=EE_T_O_pose)
gp_val = np.array([0,0,.125]).reshape((3,1))
gp = HLParam("gp", (3, 1), is_var=False, value=gp_val)
# start_pose = np.array([[-1.07265580e+00], [5.17875957e-01], [-1.30000000e+00], \
# [-1.08244363e+00], [-1.04143184e+00], [-9.06938766e-02], [-5.38403045e-01]])
# obj trajectory to follow
# obj_start = poseFromMatrix(tran).reshape((7,1))
obj_start = obj_pose_from_transform(tran).reshape((6,1))
end_tran = tran.copy()
end_tran[1, 3] = -0.34
obj_end = obj_pose_from_transform(end_tran).reshape((6,1))
# print "obj_start:", obj_start
# print "obj_end:", obj_end
# obj_traj = lininterp(obj_start, obj_end, 10)
obj_traj = lininterp(obj_start, obj_end, 2)
start = HLParam("start", (K, 1), is_var=False, value=start_pose)
end = HLParam("end", (K, 1), is_var=False, value=end_pose)
# obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
move = PR2ObjMove(0, hl_plan, move_env, robot, start, end, traj_val, obj, gp)
hlas = [move]
ll_prob = LLProb(hlas)
ll_prob.solve()
import ipdb; ipdb.set_trace()
