def sample_rp_from_ee(self, env, rs_param, ee_pose):
"""
Using Inverse Kinematics to solve all possible robot armpose and basepose that reaches this ee_pose
robot_pose: list of full robot pose with format of(basepose, backHeight, lArmPose, lGripper, rArmPose, rGripper)
"""
ee_pos, ee_rot = ee_pose[0], ee_pose[1]
base_raidus = np.linspace(-1, 1, num=BASE_SAMPLE_SIZE)
poses = list(itertools.product(base_raidus, base_raidus))
# sample
dummy_body = OpenRAVEBody(env, rs_param.name, PR2())
possible_robot_pose = []
for pos in poses:
bp = pos + ee_pos[:2]
vec = ee_pos[:2] - bp
vec = vec / np.linalg.norm(vec)
theta = math.atan2(vec[1], vec[0])
bp = np.array([bp[0], bp[1], theta])
dummy_body.set_pose(bp)
arm_poses = robot_body.ik_arm_pose(ee_pos, ee_rot)
for arm_pose in arm_poses:
possible_robot_pose.append(
(bp, ee_arm_poses[0], rs_param.lArmPose, rs_param.lGripper, ee_arm_poses[1:], rs_param.rGripper)
)
import ipdb
ipdb.set_trace()
dummy_body.delete()
return possible_robot_pose
def sample_rp_from_ee(self, env, rs_param, ee_pose):
"""
Using Inverse Kinematics to solve all possible robot armpose and basepose that reaches this ee_pose
robot_pose: list of full robot pose with format of(basepose, backHeight, lArmPose, lGripper, rArmPose, rGripper)
"""
ee_pos, ee_rot = ee_pose[0], ee_pose[1]
base_raidus = np.linspace(-1, 1, num=BASE_SAMPLE_SIZE)
poses = list(itertools.product(base_raidus, base_raidus))
# sample
dummy_body = OpenRAVEBody(env, rs_param.name, PR2())
possible_robot_pose = []
for pos in poses:
bp = pos + ee_pos[:2]
vec = ee_pos[:2] - bp
vec = vec / np.linalg.norm(vec)
theta = math.atan2(vec[1], vec[0])
bp = np.array([bp[0], bp[1], theta])
dummy_body.set_pose(bp)
arm_poses = robot_body.ik_arm_pose(ee_pos, ee_rot)
for arm_pose in arm_poses:
possible_robot_pose.append(
(bp, ee_arm_poses[0], rs_param.lArmPose, rs_param.lGripper,
ee_arm_poses[1:], rs_param.rGripper))
import ipdb
ipdb.set_trace()
dummy_body.delete()
return possible_robot_pose
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
