def evaluate(self, environment, observation):
"""
This function evaluates an observation for the assigned constraint of the class. It differentiates
between Sawyer (end-effector) and general items (blocks etc,.).
Parameters
----------
environment : Environment
The Environment object containing the current demonstrations environment (SawyerRobot, Items, Constraints)
and helper methods.
observation : Observation
The observation to evaluate for the constraint.
Returns
-------
: int
Integer value of constraint evaluation for the height constraint.
"""
if self.item_id == environment.get_robot_info()["id"]:
item_data = observation.get_robot_data()
item_pose = convert_data_to_pose(item_data["position"],
item_data["orientation"])
else:
item_data = observation.get_item_data(self.item_id)
item_pose = convert_data_to_pose(item_data["position"],
item_data["orientation"])
return planar(item_pose,
self.reference_position,
self.threshold_distance,
direction=self.direction,
axis=self.axis)
def _evaluate_within_perimeter(self, curr_observation):
for target_item_id in self.item_ids:
within_perimeter = []
perimeter = curr_observation.get_item_data(target_item_id).get(
"perimeter", None)
if perimeter is not None:
# robot first
robot_pose = convert_data_to_pose(
curr_observation.get_robot_data()["position"],
curr_observation.get_robot_data()["orientation"])
if perimeter_2D(robot_pose, perimeter["inner"],
perimeter["outer"]):
within_perimeter.append(self.robot_id)
# now all items
for item_id in self.item_ids:
if target_item_id != item_id:
item_pose = convert_data_to_pose(
curr_observation.get_item_data(
item_id)["position"],
curr_observation.get_item_data(item_id)
["orientation"])
if perimeter_2D(item_pose, perimeter["inner"],
perimeter["outer"]):
within_perimeter.append(target_id)
curr_observation.get_item_data(
target_item_id)['in_perimeter'] = within_perimeter
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument('-p',
'--position',
dest='position',
nargs=3,
type=float,
help='the x, y, z position of the end-effector')
parser.add_argument(
'-o',
'--orientation',
dest='orientation',
nargs=4,
type=float,
help='the x, y, z, w quaternion orientation of the end-effector')
args = parser.parse_args(rospy.myargv()[1:])
print(args.position)
""" Create the moveit_interface """
moveit_interface = SawyerMoveitInterface()
pose = convert_data_to_pose(position=args.position,
orientation=args.orientation)
print(moveit_interface.get_IK_pose(pose))
def _get_poses_with_reference(self, environment, observation):
if self.above_item_id == int(environment.get_robot_info()["id"]):
above_data = observation.get_robot_data()
above_pose = convert_data_to_pose(above_data["position"],
above_data["orientation"])
below_pose = convert_data_to_pose(
self.reference_pose["position"],
self.reference_pose["orientation"])
else:
above_data = observation.get_item_data(self.above_item_id)
above_pose = convert_data_to_pose(above_data["position"],
above_data["orientation"])
below_pose = convert_data_to_pose(
self.reference_pose["position"],
self.reference_pose["orientation"])
return above_pose, below_pose
def evaluate(self, environment, observation):
"""
This function evaluates an observation for the assigned constraint of the class. It differentiates
between Sawyer (end-effector) and general items (blocks etc,.).
Parameters
----------
environment : Environment
The Environment object containing the current demonstrations environment (SawyerRobot, Items, Constraints)
and helper methods.
observation : Observation
The observation to evaluate for the constraint.
Returns
-------
: int
Integer value of constraint evaluation for the perimeter_2D constraint.
"""
if self.traversing_item_id == int(environment.get_robot_info()["id"]):
traversing_item_data = observation.get_robot_data()
traversing_item_pose = convert_data_to_pose(
traversing_item_data["position"],
traversing_item_data["orientation"])
perimeter_item_data = observation.get_item_data(
self.perimeter_item_id)
inner_poly = perimeter_item_data['perimeter']['inner']
outer_poly = perimeter_item_data['perimeter']['outer']
else:
traversing_item_data = observation.get_item_data(
self.traversing_item_id)
traversing_item_pose = convert_data_to_pose(
traversing_item_data["position"],
traversing_item_data["orientation"])
perimeter_item_data = observation.get_item_data(
self.perimeter_item_id)
inner_poly = perimeter_item_data['perimeter']['inner']
outer_poly = perimeter_item_data['perimeter']['outer']
return perimeter_2D(traversing_item_pose,
inner_poly,
outer_poly,
axis=self.axis)
def evaluate(self, environment, observation):
"""
This function evaluates an observation for the assigned constraint of the class. It differentiates
between Sawyer (end-effector) and general items (blocks etc,.).
Parameters
----------
environment : Environment
The Environment object containing the current demonstrations environment (SawyerRobot, Items, Constraints)
and helper methods.
observation : Observation
The observation to evaluate for the constraint.
Returns
-------
: int
Integer value of constraint evaluation for the associate constraint and item.
"""
if self.item_id == environment.get_robot_info()["id"]:
item_data = observation.get_robot_data()
item_info = environment.get_robot_info()
else:
item_data = observation.get_item_data(self.item_id)
item_info = environment.get_item_info(self.item_id)
current_pose = convert_data_to_pose(item_data["position"],
item_data["orientation"])
if self.reference_orientation is None:
upright_pose = convert_data_to_pose(
item_info["upright_pose"]["position"],
item_info["upright_pose"]["orientation"])
else:
upright_pose = convert_data_to_pose([0, 0, 0],
self.reference_orientation)
cone_eval = cone(upright_pose, current_pose, self.threshold_angle,
self.axis)
twist_eval = twist(upright_pose, current_pose, self.threshold_angle,
self.axis)
if cone_eval and twist_eval:
return 1
else:
return 0
def plan_cartesian_path(self, scale=1):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
move_group = self.move_group
# BEGIN_SUB_TUTORIAL plan_cartesian_path
##
# Cartesian Paths
# ^^^^^^^^^^^^^^^
# You can plan a Cartesian path directly by specifying a list of waypoints
# for the end-effector to go through. If executing interactively in a
# Python shell, set scale = 1.0.
##
waypoints = []
wpose = convert_data_to_pose(self.sawyer.get_state()['position'], self.sawyer.get_state()['orientation'])
wpose.position.z -= scale * 0.1 # First move up (z)
wpose.position.y += scale * 0.2 # and sideways (y)
waypoints.append(copy.deepcopy(wpose))
wpose.position.x += scale * 0.1 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
wpose.position.y -= scale * 0.1 # Third move sideways (y)
waypoints.append(copy.deepcopy(wpose))
# We want the Cartesian path to be interpolated at a resolution of 1 cm
# which is why we will specify 0.01 as the eef_step in Cartesian
# translation. We will disable the jump threshold by setting it to 0.0,
# ignoring the check for infeasible jumps in joint space, which is sufficient
# for this tutorial.
(plan, fraction) = move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
# Note: We are just planning, not asking move_group to actually move the robot yet:
return plan, fraction
def go_to_pose_goal(self):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
move_group = self.move_group
# BEGIN_SUB_TUTORIAL plan_to_pose
##
# Planning to a Pose Goal
# ^^^^^^^^^^^^^^^^^^^^^^^
# We can plan a motion for this group to a desired pose for the
# end-effector:
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 0.4
pose_goal.position.y = 0.1
pose_goal.position.z = 0.4
move_group.set_pose_target(pose_goal)
# Now, we call the planner to compute the plan and execute it.
plan = move_group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
move_group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
move_group.clear_pose_targets()
# END_SUB_TUTORIAL
# For testing:
# Note that since this section of code will not be included in the tutorials
# we use the class variable rather than the copied state variable
current_pose = convert_data_to_pose(self.sawyer.get_state()['position'], self.sawyer.get_state()['orientation'])
return all_close(pose_goal, current_pose, 0.01)