def test_ik_arm_pose(self):
env = Environment()
attrs = {"name": ["pr2"], "pose": [(0, 0, 0)], "_type": ["Robot"], "geom": [], "backHeight": [0.2], "lGripper": [0.5], "rGripper": [0.5]}
attrs["lArmPose"] = [(0,0,0,0,0,0,0)]
attrs["rArmPose"] = [(0,0,0,0,0,0,0)]
attr_types = {"name": str, "pose": matrix.Vector3d, "_type": str, "geom": PR2, "backHeight": matrix.Value, "lArmPose": matrix.Vector7d, "rArmPose": matrix.Vector7d, "lGripper": matrix.Value, "rGripper": matrix.Value}
robot = parameter.Object(attrs, attr_types)
# Set the initial arm pose so that pose is not close to joint limit
robot.lArmPose = np.array([[np.pi/4, np.pi/8, np.pi/2, -np.pi/2, np.pi/8, -np.pi/8, np.pi/2]]).T
robot.rArmPose = np.array([[-np.pi/4, np.pi/8, -np.pi/2, -np.pi/2, -np.pi/8, -np.pi/8, np.pi/2]]).T
attrs = {"name": ["can"], "geom": (0.04, 0.25), "pose": ["undefined"], "rotation": [(0, 0, 0)], "_type": ["Can"]}
attr_types = {"name": str, "geom": BlueCan, "pose": matrix.Vector3d, "rotation": matrix.Vector3d, "_type": str}
can = parameter.Object(attrs, attr_types)
can.pose = np.array([[5.77887566e-01, -1.26743678e-01, 8.37601627e-01]]).T
can.rotation = np.array([[np.pi/4, np.pi/4, np.pi/4]]).T
# Create openRAVEBody for each parameter
can_body = OpenRAVEBody(env, can.name, can.geom)
robot_body = OpenRAVEBody(env, robot.name, robot.geom)
# Set the poses and dof values for each body
can_body.set_pose(can.pose, can.rotation)
robot_body.set_pose(robot.pose)
robot_body.set_dof(robot.backHeight, robot.lArmPose, robot.lGripper, robot.rArmPose, robot.rGripper)
# Solve the IK solution
ik_arm = robot_body.ik_arm_pose(can.pose, can.rotation)[0]
robot_body.set_dof(ik_arm[:1], robot.lArmPose, robot.lGripper, ik_arm[1:], robot.rGripper)
robot_trans = robot_body.env_body.GetLink("r_gripper_tool_frame").GetTransform()
robot_pos = OpenRAVEBody.obj_pose_from_transform(robot_trans)
# resulted robot eepose should be exactly the same as that can pose
self.assertTrue(np.allclose(can.pose.flatten(), robot_pos[:3], atol = 1e-4))
self.assertTrue(np.allclose(can.rotation.flatten(), robot_pos[3:], atol = 1e-4))
"""
def sample_rp_from_ee(t, plan, ee_poses, robot):
target_pose = target.value[:, 0]
dummy_body = OpenRAVEBody(plan.env, robot.name+'_dummy', robot.geom)
possible_rps = []
for ee_pose in ee_poses:
base_pose = sampling.get_col_free_base_pose_around_target(t, plan, ee_pose.value, robot).reshape((1,3))
ik_arm_poses = dummy_body.ik_arm_pose(ee_pose[0], ee_pose[1])
cur_arm_poses = np.c_[robot.backHeight, robot.rArmPose].reshape((8,))
chosen_pose = sampling.closest_arm_pose(ik_arm_poses, cur_arm_poses)
torso_pose, arm_pose = chosen_pose[0], chosen_pose[1:]
possible_rp = np.hstack((base_pose, torso_pose, robot.lArmPose[:,t], robot.lGripper[:,t], rArmPose, robot.rGripper[:,t]))
possible_rps.append(possible_rp)
dummy_body.delete()
if len(possible_rps) <= 0:
import ipdb; ipdb.set_trace()
return possible_rps
def sample_rp_from_ee(t, plan, ee_poses, robot):
target_pose = target.value[:, 0]
dummy_body = OpenRAVEBody(plan.env, robot.name + '_dummy', robot.geom)
possible_rps = []
for ee_pose in ee_poses:
base_pose = sampling.get_col_free_base_pose_around_target(
t, plan, ee_pose.value, robot).reshape((1, 3))
ik_arm_poses = dummy_body.ik_arm_pose(ee_pose[0], ee_pose[1])
cur_arm_poses = np.c_[robot.backHeight, robot.rArmPose].reshape(
(8, ))
chosen_pose = sampling.closest_arm_pose(ik_arm_poses,
cur_arm_poses)
torso_pose, arm_pose = chosen_pose[0], chosen_pose[1:]
possible_rp = np.hstack(
(base_pose, torso_pose, robot.lArmPose[:, t],
robot.lGripper[:, t], rArmPose, robot.rGripper[:, t]))
possible_rps.append(possible_rp)
dummy_body.delete()
if len(possible_rps) <= 0:
print "something went wrong"
# import ipdb; ipdb.set_trace()
return possible_rps
def test_ik_arm_pose(self):
env = Environment()
attrs = {
"name": ["pr2"],
"pose": [(0, 0, 0)],
"_type": ["Robot"],
"geom": [],
"backHeight": [0.2],
"lGripper": [0.5],
"rGripper": [0.5]
}
attrs["lArmPose"] = [(0, 0, 0, 0, 0, 0, 0)]
attrs["rArmPose"] = [(0, 0, 0, 0, 0, 0, 0)]
attr_types = {
"name": str,
"pose": matrix.Vector3d,
"_type": str,
"geom": PR2,
"backHeight": matrix.Value,
"lArmPose": matrix.Vector7d,
"rArmPose": matrix.Vector7d,
"lGripper": matrix.Value,
"rGripper": matrix.Value
}
robot = parameter.Object(attrs, attr_types)
# Set the initial arm pose so that pose is not close to joint limit
robot.lArmPose = np.array([[
np.pi / 4, np.pi / 8, np.pi / 2, -np.pi / 2, np.pi / 8, -np.pi / 8,
np.pi / 2
]]).T
robot.rArmPose = np.array([[
-np.pi / 4, np.pi / 8, -np.pi / 2, -np.pi / 2, -np.pi / 8,
-np.pi / 8, np.pi / 2
]]).T
attrs = {
"name": ["can"],
"geom": (0.04, 0.25),
"pose": ["undefined"],
"rotation": [(0, 0, 0)],
"_type": ["Can"]
}
attr_types = {
"name": str,
"geom": BlueCan,
"pose": matrix.Vector3d,
"rotation": matrix.Vector3d,
"_type": str
}
can = parameter.Object(attrs, attr_types)
can.pose = np.array([[5.77887566e-01, -1.26743678e-01,
8.37601627e-01]]).T
can.rotation = np.array([[np.pi / 4, np.pi / 4, np.pi / 4]]).T
# Create openRAVEBody for each parameter
can_body = OpenRAVEBody(env, can.name, can.geom)
robot_body = OpenRAVEBody(env, robot.name, robot.geom)
# Set the poses and dof values for each body
can_body.set_pose(can.pose, can.rotation)
robot_body.set_pose(robot.pose)
robot_body.set_dof(robot.backHeight, robot.lArmPose, robot.lGripper,
robot.rArmPose, robot.rGripper)
# Solve the IK solution
ik_arm = robot_body.ik_arm_pose(can.pose, can.rotation)[0]
robot_body.set_dof(ik_arm[:1], robot.lArmPose, robot.lGripper,
ik_arm[1:], robot.rGripper)
robot_trans = robot_body.env_body.GetLink(
"r_gripper_tool_frame").GetTransform()
robot_pos = OpenRAVEBody.obj_pose_from_transform(robot_trans)
# resulted robot eepose should be exactly the same as that can pose
self.assertTrue(
np.allclose(can.pose.flatten(), robot_pos[:3], atol=1e-4))
self.assertTrue(
np.allclose(can.rotation.flatten(), robot_pos[3:], atol=1e-4))
"""
