added warning to SetPredictionHorizon

src/giskardpy/goals/set_prediction_horizon.py

@@ -1,15 +1,28 @@
 from giskardpy import identifier
 from giskardpy.goals.goal import Goal
+from giskardpy.utils import logging
 
 
 class SetPredictionHorizon(Goal):
- def __init__(self, prediction_horizon, joint_limits=None, joint_weights=None, **kwargs):
+ def __init__(self, prediction_horizon, **kwargs):
  super(SetPredictionHorizon, self).__init__(**kwargs)
  self.prediction_horizon = prediction_horizon
+ if 5 > self.prediction_horizon > 1:
+ logging.logwarn('Prediction horizon should be 1 or greater equal 5.')
  if self.prediction_horizon == 1:
- del self.god_map.get_data(identifier.joint_weights)['acceleration']
- del self.god_map.get_data(identifier.joint_limits)['acceleration']
- del self.god_map.get_data(identifier.joint_weights)['jerk']
- del self.god_map.get_data(identifier.joint_limits)['jerk']
+ if 'acceleration' in self.god_map.get_data(identifier.joint_weights):
+ del self.god_map.get_data(identifier.joint_weights)['acceleration']
+ if 'acceleration' in self.god_map.get_data(identifier.joint_limits):
+ del self.god_map.get_data(identifier.joint_limits)['acceleration']
+ if self.prediction_horizon <= 2:
+ if 'jerk' in self.god_map.get_data(identifier.joint_weights):
+ del self.god_map.get_data(identifier.joint_weights)['jerk']
+ if 'jerk' in self.god_map.get_data(identifier.joint_limits):
+ del self.god_map.get_data(identifier.joint_limits)['jerk']
+ if self.prediction_horizon <= 3:
+ if 'jerk' in self.god_map.get_data(identifier.joint_weights):
+ del self.god_map.get_data(identifier.joint_weights)['snap']
+ if 'jerk' in self.god_map.get_data(identifier.joint_limits):
+ del self.god_map.get_data(identifier.joint_limits)['snap']
  self.god_map.set_data(identifier.prediction_horizon, prediction_horizon)
  self.world.sync_with_paramserver()

test/test_integration_pr2.py

@@ -5049,6 +5049,148 @@ def test_prediction_horizon1(self, zero_pose):
  zero_pose.set_joint_goal(default_pose)
  zero_pose.plan_and_execute()
 
+ def test_bowl_and_cup_prediction_horizon1(self, kitchen_setup):
+ """
+ :type kitchen_setup: PR2
+ :return:
+ """
+ # kernprof -lv py.test -s test/test_integration_pr2.py::TestCollisionAvoidanceGoals::test_bowl_and_cup
+ bowl_name = u'bowl'
+ cup_name = u'cup'
+ percentage = 50
+ drawer_handle = u'sink_area_left_middle_drawer_handle'
+ drawer_joint = u'sink_area_left_middle_drawer_main_joint'
+
+ kitchen_setup.set_json_goal('SetPredictionHorizon', prediction_horizon=1)
+
+ # grasp drawer handle
+ bar_axis = Vector3Stamped()
+ bar_axis.header.frame_id = drawer_handle
+ bar_axis.vector.y = 1
+
+ bar_center = PointStamped()
+ bar_center.header.frame_id = drawer_handle
+
+ tip_grasp_axis = Vector3Stamped()
+ tip_grasp_axis.header.frame_id = kitchen_setup.l_tip
+ tip_grasp_axis.vector.z = 1
+
+ kitchen_setup.set_json_goal(u'GraspBar',
+ root_link=kitchen_setup.default_root,
+ tip_link=kitchen_setup.l_tip,
+ tip_grasp_axis=tip_grasp_axis,
+ bar_center=bar_center,
+ bar_axis=bar_axis,
+ bar_length=0.4) # TODO: check for real length
+ x_gripper = Vector3Stamped()
+ x_gripper.header.frame_id = kitchen_setup.l_tip
+ x_gripper.vector.x = 1
+
+ x_goal = Vector3Stamped()
+ x_goal.header.frame_id = drawer_handle
+ x_goal.vector.x = -1
+
+ kitchen_setup.set_align_planes_goal(kitchen_setup.l_tip,
+ x_gripper,
+ root_normal=x_goal)
+ # kitchen_setup.allow_all_collisions()
+ kitchen_setup.plan_and_execute()
+ kitchen_setup.set_json_goal('SetPredictionHorizon', prediction_horizon=1)
+
+ # open drawer
+ kitchen_setup.set_json_goal(u'Open',
+ tip_link=kitchen_setup.l_tip,
+ environment_link=drawer_handle)
+ kitchen_setup.plan_and_execute()
+ kitchen_setup.set_json_goal('SetPredictionHorizon', prediction_horizon=1)
+ kitchen_setup.set_kitchen_js({drawer_joint: 0.48})
+
+ # spawn cup
+ cup_pose = PoseStamped()
+ cup_pose.header.frame_id = u'iai_kitchen/sink_area_left_middle_drawer_main'
+ cup_pose.pose.position = Point(0.1, 0.2, -.05)
+ cup_pose.pose.orientation = Quaternion(0, 0, 0, 1)
+
+ kitchen_setup.add_cylinder(cup_name, height=0.07, radius=0.04, pose=cup_pose)
+
+ # spawn bowl
+ bowl_pose = PoseStamped()
+ bowl_pose.header.frame_id = u'iai_kitchen/sink_area_left_middle_drawer_main'
+ bowl_pose.pose.position = Point(0.1, -0.2, -.05)
+ bowl_pose.pose.orientation = Quaternion(0, 0, 0, 1)
+
+ kitchen_setup.add_cylinder(bowl_name, height=0.05, radius=0.07, pose=bowl_pose)
+ kitchen_setup.set_joint_goal(gaya_pose)
+ kitchen_setup.plan_and_execute()
+ kitchen_setup.set_json_goal('SetPredictionHorizon', prediction_horizon=1)
+
+ # grasp bowl
+ l_goal = deepcopy(bowl_pose)
+ l_goal.pose.position.z += .2
+ l_goal.pose.orientation = Quaternion(*quaternion_from_matrix([[0, 1, 0, 0],
+ [0, 0, -1, 0],
+ [-1, 0, 0, 0],
+ [0, 0, 0, 1]]))
+ kitchen_setup.set_cart_goal(l_goal, kitchen_setup.l_tip, kitchen_setup.default_root)
+
+ # grasp cup
+ r_goal = deepcopy(cup_pose)
+ r_goal.pose.position.z += .2
+ r_goal.pose.orientation = Quaternion(*quaternion_from_matrix([[0, 1, 0, 0],
+ [0, 0, -1, 0],
+ [-1, 0, 0, 0],
+ [0, 0, 0, 1]]))
+ kitchen_setup.set_json_goal(u'AvoidJointLimits', percentage=percentage)
+ kitchen_setup.set_cart_goal(r_goal, kitchen_setup.r_tip, kitchen_setup.default_root)
+ kitchen_setup.plan_and_execute()
+ kitchen_setup.set_json_goal('SetPredictionHorizon', prediction_horizon=1)
+
+ l_goal.pose.position.z -= .2
+ r_goal.pose.position.z -= .2
+ kitchen_setup.allow_collision([CollisionEntry.ALL], bowl_name, [CollisionEntry.ALL])
+ kitchen_setup.allow_collision([CollisionEntry.ALL], cup_name, [CollisionEntry.ALL])
+ kitchen_setup.set_cart_goal(l_goal, kitchen_setup.l_tip, kitchen_setup.default_root)
+ kitchen_setup.set_cart_goal(r_goal, kitchen_setup.r_tip, kitchen_setup.default_root)
+ kitchen_setup.set_json_goal(u'AvoidJointLimits', percentage=percentage)
+ kitchen_setup.plan_and_execute()
+ kitchen_setup.set_json_goal('SetPredictionHorizon', prediction_horizon=1)
+
+ kitchen_setup.attach_object(bowl_name, kitchen_setup.l_tip)
+ kitchen_setup.attach_object(cup_name, kitchen_setup.r_tip)
+
+ kitchen_setup.set_joint_goal(gaya_pose)
+ kitchen_setup.plan_and_execute()
+ kitchen_setup.set_json_goal('SetPredictionHorizon', prediction_horizon=1)
+ base_goal = PoseStamped()
+ base_goal.header.frame_id = u'base_footprint'
+ base_goal.pose.position.x = -.1
+ base_goal.pose.orientation = Quaternion(*quaternion_about_axis(pi, [0, 0, 1]))
+ kitchen_setup.teleport_base(base_goal)
+
+ # place bowl and cup
+ bowl_goal = PoseStamped()
+ bowl_goal.header.frame_id = u'iai_kitchen/kitchen_island_surface'
+ bowl_goal.pose.position = Point(.2, 0, .05)
+ bowl_goal.pose.orientation = Quaternion(0, 0, 0, 1)
+
+ cup_goal = PoseStamped()
+ cup_goal.header.frame_id = u'iai_kitchen/kitchen_island_surface'
+ cup_goal.pose.position = Point(.15, 0.25, .07)
+ cup_goal.pose.orientation = Quaternion(0, 0, 0, 1)
+
+ kitchen_setup.set_cart_goal(bowl_goal, bowl_name, kitchen_setup.default_root)
+ kitchen_setup.set_cart_goal(cup_goal, cup_name, kitchen_setup.default_root)
+ kitchen_setup.set_json_goal(u'AvoidJointLimits', percentage=percentage)
+ kitchen_setup.plan_and_execute()
+ kitchen_setup.set_json_goal('SetPredictionHorizon', prediction_horizon=1)
+
+ kitchen_setup.detach_object(bowl_name)
+ kitchen_setup.detach_object(cup_name)
+ kitchen_setup.allow_collision([], cup_name, [])
+ kitchen_setup.allow_collision([], bowl_name, [])
+ kitchen_setup.set_joint_goal(gaya_pose)
+ kitchen_setup.plan_and_execute()
+
 # import pytest
 # pytest.main(['-s', __file__ + '::TestJointGoals::test_joint_movement1'])
 # pytest.main(['-s', __file__ + '::TestCollisionAvoidanceGoals::test_bowl_and_cup'])