def call_tsr_plan_service(whole_body, constraint_tsrs, goal_tsrs):
odom_to_robot_pose = geometry.tuples_to_pose(
get_relative_tuples(_ORIGIN_TF, _ROBOT_TF))
req = PlanWithTsrConstraintsRequest()
req.base_movement_type.val = BaseMovementType.PLANAR
req.origin_to_basejoint = odom_to_robot_pose
req.initial_joint_state = whole_body._get_joint_state()
req.use_joints = _USE_JOINTS
req.probability_goal_generate = _PLANNING_GOAL_GENERATION
req.timeout = rospy.Duration(whole_body._planning_timeout)
req.max_iteration = _PLANNING_MAX_ITERATION
req.uniform_bound_sampling = False
req.deviation_for_bound_sampling = _PLANNING_GOAL_DEVIATION
req.weighted_joints = ['_linear_base', '_rotational_base']
req.weight = [whole_body._linear_weight, whole_body._angular_weight]
if whole_body._collision_world is not None:
req.environment_before_planning = whole_body._collision_world.snapshot(
_ORIGIN_TF)
req.constraint_tsrs = constraint_tsrs
req.goal_tsrs = goal_tsrs
plan_service = rospy.ServiceProxy('plan_with_constraints',
PlanWithTsrConstraints)
res = plan_service.call(req)
return res
def _plan_cartesian_path(self, origin_to_pose1, origin_to_pose2,
odom_to_robot_pose, initial_joint_state,
collision_env):
use_joints = (b'wrist_flex_joint', b'wrist_roll_joint',
b'arm_roll_joint', b'arm_flex_joint', b'arm_lift_joint')
req = PlanWithTsrConstraintsRequest()
req.origin_to_basejoint = odom_to_robot_pose
req.initial_joint_state = initial_joint_state
req.base_movement_type.val = BaseMovementType.PLANAR
req.use_joints = use_joints
req.weighted_joints = [b'_linear_base', b'_rotational_base']
req.weight = [self._linear_weight, self._angular_weight]
req.probability_goal_generate = _PLANNING_GOAL_GENERATION
req.attached_objects = []
if collision_env is not None:
req.environment_before_planning = collision_env
req.timeout = rospy.Duration(self._planning_timeout)
req.max_iteration = _PLANNING_MAX_ITERATION
req.uniform_bound_sampling = False
req.deviation_for_bound_sampling = _PLANNING_GOAL_DEVIATION
req.extra_constraints = []
req.extra_goal_constraints = []
move_axis, distance = _movement_axis_and_distance(
origin_to_pose1, origin_to_pose2)
origin_to_axis = _pose_from_x_axis(move_axis)
pose1_to_axis = geometry.multiply_tuples(_invert_pose(origin_to_pose1),
origin_to_axis)
pose1_to_axis = geometry.Pose((0, 0, 0), pose1_to_axis[1])
origin_to_tsr = geometry.multiply_tuples(origin_to_pose1,
pose1_to_axis)
tsr_to_pose1 = _invert_pose(pose1_to_axis)
# Goal constraint
tsr_g = TaskSpaceRegion()
tsr_g.end_frame_id = bytes(self.end_effector_frame)
tsr_g.origin_to_tsr = geometry.tuples_to_pose(origin_to_pose2)
tsr_g.tsr_to_end = geometry.tuples_to_pose(geometry.pose())
tsr_g.min_bounds = [0, 0, 0, 0, 0, 0]
tsr_g.max_bounds = [0, 0, 0, 0, 0, 0]
# Line constraint
tsr_c = TaskSpaceRegion()
tsr_c.end_frame_id = bytes(self.end_effector_frame)
tsr_c.origin_to_tsr = geometry.tuples_to_pose(origin_to_tsr)
tsr_c.tsr_to_end = geometry.tuples_to_pose(tsr_to_pose1)
tsr_c.min_bounds = [0, 0, 0, -math.pi, -math.pi, -math.pi]
tsr_c.max_bounds = [distance, 0, 0, math.pi, math.pi, math.pi]
req.goal_tsrs = [tsr_g]
req.constraint_tsrs = [tsr_c]
service_name = self._setting['plan_with_constraints_service']
plan_service = rospy.ServiceProxy(service_name, PlanWithTsrConstraints)
res = plan_service.call(req)
print(res)
if res.error_code.val != ArmManipulationErrorCodes.SUCCESS:
msg = "Fail to plan"
print(msg)
print(req)
raise exceptions.MotionPlanningError(msg, res.error_code)
return res
