more tests and refactoring

launch/giskardpy_pr2.launch

@@ -16,7 +16,7 @@
  <rosparam param="nWSR">None</rosparam> <!-- None results in a nWSR estimation thats fine most of the time -->
  <rosparam param="root_link">base_footprint</rosparam>
  <rosparam param="enable_collision_marker">True</rosparam>
- <rosparam param="enable_self_collision">False</rosparam>
+ <rosparam param="enable_self_collision">True</rosparam>
  <rosparam param="collision_time_threshold">15</rosparam>
  <rosparam param="interactive_marker_chains">
  - [base_link, r_gripper_tool_frame]

scripts/ros_trajectory_controller_main.py

@@ -19,53 +19,53 @@ def giskard_pm():
  # TODO 0 0 0 in base footprint as goal results in /0
  # TODO bug if first goal is joint
  # root_link = rospy.get_param('~root_link', 'odom')
- root_tips = rospy.get_param('~interactive_marker_chains')
- gui = rospy.get_param('~enable_gui')
- map_frame = rospy.get_param('~map_frame')
- joint_convergence_threshold = rospy.get_param('~joint_convergence_threshold')
- wiggle_precision_threshold = rospy.get_param('~wiggle_precision_threshold')
- sample_period = rospy.get_param('~sample_period')
- default_joint_vel_limit = rospy.get_param('~default_joint_vel_limit')
- default_collision_avoidance_distance = rospy.get_param('~default_collision_avoidance_distance')
- fill_velocity_values = rospy.get_param('~fill_velocity_values')
- nWSR = rospy.get_param('~nWSR')
- root_link = rospy.get_param('~root_link')
- marker = rospy.get_param('~enable_collision_marker')
- enable_self_collision = rospy.get_param('~enable_self_collision')
- if nWSR == 'None':
+ root_tips = rospy.get_param(u'~interactive_marker_chains')
+ gui = rospy.get_param(u'~enable_gui')
+ map_frame = rospy.get_param(u'~map_frame')
+ joint_convergence_threshold = rospy.get_param(u'~joint_convergence_threshold')
+ wiggle_precision_threshold = rospy.get_param(u'~wiggle_precision_threshold')
+ sample_period = rospy.get_param(u'~sample_period')
+ default_joint_vel_limit = rospy.get_param(u'~default_joint_vel_limit')
+ default_collision_avoidance_distance = rospy.get_param(u'~default_collision_avoidance_distance')
+ fill_velocity_values = rospy.get_param(u'~fill_velocity_values')
+ nWSR = rospy.get_param(u'~nWSR')
+ root_link = rospy.get_param(u'~root_link')
+ marker = rospy.get_param(u'~enable_collision_marker')
+ enable_self_collision = rospy.get_param(u'~enable_self_collision')
+ if nWSR == u'None':
  nWSR = None
- path_to_data_folder = rospy.get_param('~path_to_data_folder')
- collision_time_threshold = rospy.get_param('~collision_time_threshold')
- max_traj_length = rospy.get_param('~max_traj_length')
+ path_to_data_folder = rospy.get_param(u'~path_to_data_folder')
+ collision_time_threshold = rospy.get_param(u'~collision_time_threshold')
+ max_traj_length = rospy.get_param(u'~max_traj_length')
  # path_to_data_folder = '/home/ichumuh/giskardpy_ws/src/giskardpy/data/pr2'
- if not path_to_data_folder.endswith('/'):
- path_to_data_folder += '/'
+ if not path_to_data_folder.endswith(u'/'):
+ path_to_data_folder += u'/'
 
- fk_identifier = 'fk'
- cartesian_goal_identifier = 'goal'
- js_identifier = 'js'
- controlled_joints_identifier = 'controlled_joints'
- trajectory_identifier = 'traj'
- time_identifier = 'time'
- next_cmd_identifier = 'motor'
- collision_identifier = 'collision'
- closest_point_identifier = 'cpi'
- collision_goal_identifier = 'collision_goal'
- pyfunction_identifier = 'pyfunctions'
- controllable_links_identifier = 'controllable_links'
- robot_description_identifier = 'robot_description'
+ fk_identifier = u'fk'
+ cartesian_goal_identifier = u'goal'
+ js_identifier = u'js'
+ controlled_joints_identifier = u'controlled_joints'
+ trajectory_identifier = u'traj'
+ time_identifier = u'time'
+ next_cmd_identifier = u'motor'
+ collision_identifier = u'collision'
+ closest_point_identifier = u'cpi'
+ collision_goal_identifier = u'collision_goal'
+ pyfunction_identifier = u'pyfunctions'
+ controllable_links_identifier = u'controllable_links'
+ robot_description_identifier = u'robot_description'
 
  pm = ProcessManager()
- pm.register_plugin('js',
+ pm.register_plugin(u'js',
  JointStatePlugin(js_identifier=js_identifier,
  time_identifier=time_identifier,
  next_cmd_identifier=next_cmd_identifier,
  sample_period=sample_period))
- pm.register_plugin('controlled joints',
+ pm.register_plugin(u'controlled joints',
  SetControlledJointsPlugin(controlled_joints_identifier=controlled_joints_identifier))
- pm.register_plugin('upload robot description',
+ pm.register_plugin(u'upload robot description',
  UploadRobotDescriptionPlugin(robot_description_identifier=robot_description_identifier))
- pm.register_plugin('action server',
+ pm.register_plugin(u'action server',
  ActionServerPlugin(js_identifier=js_identifier,
  trajectory_identifier=trajectory_identifier,
  cartesian_goal_identifier=cartesian_goal_identifier,
@@ -80,7 +80,7 @@ def giskard_pm():
  fill_velocity_values=fill_velocity_values,
  collision_time_threshold=collision_time_threshold,
  max_traj_length=max_traj_length))
- pm.register_plugin('bullet',
+ pm.register_plugin(u'bullet',
  PyBulletPlugin(js_identifier=js_identifier,
  collision_identifier=collision_identifier,
  closest_point_identifier=closest_point_identifier,
@@ -94,10 +94,10 @@ def giskard_pm():
  default_collision_avoidance_distance=default_collision_avoidance_distance,
  enable_self_collision=enable_self_collision,
  robot_description_identifier=robot_description_identifier))
- pm.register_plugin('fk', FKPlugin(js_identifier=js_identifier,
+ pm.register_plugin(u'fk', FKPlugin(js_identifier=js_identifier,
  fk_identifier=fk_identifier,
  robot_description_identifier=robot_description_identifier))
- pm.register_plugin('cart bullet controller',
+ pm.register_plugin(u'cart bullet controller',
  PluginContainer(
  CartesianBulletControllerPlugin(root_link=root_link,
  fk_identifier=fk_identifier,
@@ -114,12 +114,12 @@ def giskard_pm():
  nWSR=nWSR,
  default_joint_vel_limit=default_joint_vel_limit,
  robot_description_identifier=robot_description_identifier)))
- pm.register_plugin('interactive marker',
+ pm.register_plugin(u'interactive marker',
  InteractiveMarkerPlugin(root_tips=root_tips))
  return pm
 
-if __name__ == '__main__':
- rospy.init_node('giskard')
+if __name__ == u'__main__':
+ rospy.init_node(u'giskard')
  app = ROSApplication(giskard_pm())
  app.run()
 

src/giskardpy/object.py

@@ -1,7 +1,9 @@
+from giskard_msgs.srv import UpdateWorldRequest
+from std_msgs.msg import ColorRGBA
 from visualization_msgs.msg import MarkerArray, Marker
 from giskard_msgs.msg import WorldBody
 from shape_msgs.msg import SolidPrimitive
-from geometry_msgs.msg import Pose as PoseMsg, Point as PointMsg, Quaternion as QuaternionMsg
+from geometry_msgs.msg import Pose as PoseMsg, Point as PointMsg, Quaternion as QuaternionMsg, Vector3
 
 from giskardpy.exceptions import CorruptShapeException
 from giskardpy.data_types import Transform, Point, Quaternion
@@ -84,14 +86,14 @@ def __init__(self, name='', origin=Transform(), geometry=None):
  self.geometry = geometry
 
 
-class WorldObject(object):
+class UrdfObject(object):
  def __init__(self, name='', inertial_props=None, visual_props=(), collision_props=()):
  self.name = name
  self.inertial_props = inertial_props
  self.visual_props = visual_props
  self.collision_props = collision_props
 
-class Box(WorldObject):
+class Box(UrdfObject):
  def __init__(self, name, length, width, height):
  geom = BoxShape(length,
  width,
@@ -100,14 +102,14 @@ def __init__(self, name, length, width, height):
  vis = VisualProperty(name=name + '_vis', geometry=geom)
  super(Box, self).__init__(name, collision_props=[col], visual_props=[vis])
 
-class Sphere(WorldObject):
+class Sphere(UrdfObject):
  def __init__(self, name, radius):
  geom = SphereShape(radius)
  col = CollisionProperty(name=name + '_col', geometry=geom)
  vis = VisualProperty(name=name + '_vis', geometry=geom)
  super(Sphere, self).__init__(name, collision_props=[col], visual_props=[vis])
 
-class Cylinder(WorldObject):
+class Cylinder(UrdfObject):
  def __init__(self, name, radius, length):
  geom = CylinderShape(radius, length)
  col = CollisionProperty(name=name + '_col', geometry=geom)
@@ -132,7 +134,7 @@ def to_urdf_xml(urdf_object, skip_robot_tag=False):
  :return:
  :rtype: lxml.etree.Element
  """
- if isinstance(urdf_object, WorldObject):
+ if isinstance(urdf_object, UrdfObject):
  link = etree.Element('link', name=urdf_object.name)
  if urdf_object.inertial_props:
  link.append(to_urdf_xml(urdf_object.inertial_props))
@@ -213,13 +215,80 @@ def to_urdf_xml(urdf_object, skip_robot_tag=False):
 def to_urdf_string(urdf_object, skip_robot_tag=False):
  """
  :param urdf_object:
- :type urdf_object: WorldObject
+ :type urdf_object: UrdfObject
  :return:
  :rtype: str
  """
  return etree.tostring(to_urdf_xml(urdf_object, skip_robot_tag=skip_robot_tag))
 
-def to_marker(urdf_object):
+def to_marker(thing):
+ """
+ :type thing: Union[UpdateWorldRequest, WorldBody]
+ :rtype: MarkerArray
+ """
+ ma = MarkerArray()
+ if isinstance(thing, UrdfObject):
+ pass
+ # TODO
+ # return urdf_object_to_marker_msg(thing)
+ elif isinstance(thing, WorldBody):
+ ma.markers.append(world_body_to_marker_msg(thing))
+ elif isinstance(thing, UpdateWorldRequest):
+ ma.markers.append(update_world_to_marker_msg(thing))
+ return ma
+
+def update_world_to_marker_msg(update_world_req, id=1, ns=''):
+ """
+ :type update_world_req: UpdateWorldRequest
+ :type id: int
+ :type ns: str
+ :rtype: Marker
+ """
+ m = world_body_to_marker_msg(update_world_req.body, id, ns)
+ m.header = update_world_req.pose.header
+ m.pose = update_world_req.pose.pose
+ m.frame_locked = update_world_req.rigidly_attached
+ if update_world_req.operation == UpdateWorldRequest.ADD:
+ m.action = Marker.ADD
+ elif update_world_req.operation == UpdateWorldRequest.REMOVE:
+ m.action = Marker.DELETE
+ elif update_world_req.operation == UpdateWorldRequest.REMOVE_ALL:
+ m.action = Marker.DELETEALL
+ return m
+
+
+def world_body_to_marker_msg(world_body, id=1, ns=''):
+ """
+ :type world_body: WorldBody
+ :rtype: Marker
+ """
+ m = Marker()
+ m.ns = u'{}/{}'.format(ns, world_body.name)
+ m.id = id
+ if world_body.type == WorldBody.URDF_BODY:
+ raise Exception(u'can\'t convert urdf body world object to marker array')
+ elif world_body.type == WorldBody.PRIMITIVE_BODY:
+ if world_body.shape.type == SolidPrimitive.BOX:
+ m.type = Marker.CUBE
+ elif world_body.shape.type == SolidPrimitive.SPHERE:
+ m.type = Marker.SPHERE
+ elif world_body.shape.type == SolidPrimitive.CYLINDER:
+ m.type = Marker.CYLINDER
+ else:
+ raise Exception(u'world body type {} can\'t be converted to marker'.format(world_body.shape.type))
+ elif world_body.type == WorldBody.MESH_BODY:
+ m.type = Marker.MESH_RESOURCE
+ m.mesh_resource = world_body.mesh
+ m.scale = Vector3(*world_body.shape.dimensions)
+ m.color = ColorRGBA(0,1,0,0.8)
+ return m
+
+
+def urdf_object_to_marker_msg(urdf_object):
+ """
+ :type urdf_object: UrdfObject
+ :rtype: MarkerArray
+ """
  ma = MarkerArray()
  for visual_property in urdf_object.visual_props:
  m = Marker()
@@ -253,6 +322,37 @@ def to_marker(urdf_object):
  ma.markers.append(m)
  return ma
 
+def world_body_to_urdf_object(world_body_msg):
+ """
+ Converts a body from a ROS message to the corresponding internal representation.
+ :param world_body_msg: Input message that shall be converted.
+ :type world_body_msg: WorldBody
+ :return: Internal representation of body, filled with data from input message.
+ :rtype UrdfObject
+ """
+ if world_body_msg.type is WorldBody.MESH_BODY:
+ geom = MeshShape(filename=world_body_msg.mesh)
+ elif world_body_msg.type is WorldBody.PRIMITIVE_BODY:
+ if world_body_msg.shape.type is SolidPrimitive.BOX:
+ geom = BoxShape(world_body_msg.shape.dimensions[SolidPrimitive.BOX_X],
+ world_body_msg.shape.dimensions[SolidPrimitive.BOX_Y],
+ world_body_msg.shape.dimensions[SolidPrimitive.BOX_Z])
+ elif world_body_msg.shape.type is SolidPrimitive.CYLINDER:
+ geom = CylinderShape(world_body_msg.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS],
+ world_body_msg.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT])
+ elif world_body_msg.shape.type is SolidPrimitive.SPHERE:
+ geom = SphereShape(world_body_msg.shape.dimensions[SolidPrimitive.SPHERE_RADIUS])
+ else:
+ raise CorruptShapeException("Invalid primitive shape '{}' of world body '{}'".format(world_body_msg.shape.type, world_body_msg.name))
+ elif world_body_msg.type is WorldBody.URDF_BODY:
+ # TODO: complete me
+ pass
+ else:
+ # TODO: replace me by a proper exception that can be reported back to the service client
+ raise RuntimeError("Invalid shape of world body: {}".format(world_body_msg.shape))
+ col = CollisionProperty(name=world_body_msg.name + '_col', geometry=geom)
+ vis = VisualProperty(name=world_body_msg.name + '_vis', geometry=geom)
+ return UrdfObject(name=world_body_msg.name, collision_props=[col], visual_props=[vis])
 
 def from_point_msg(point_msg):
  """
@@ -285,36 +385,3 @@ def from_pose_msg(pose_msg):
  :rtype: Transform
  """
  return Transform(from_point_msg(pose_msg.position), from_quaternion_msg(pose_msg.orientation))
-
-
-def from_msg(body_msg):
- """
- Converts a body from a ROS message to the corresponding internal representation.
- :param body_msg: Input message that shall be converted.
- :type body_msg: WorldBody
- :return: Internal representation of body, filled with data from input message.
- :rtype WorldObject
- """
- if body_msg.type is WorldBody.MESH_BODY:
- geom = MeshShape(filename=body_msg.mesh)
- elif body_msg.type is WorldBody.PRIMITIVE_BODY:
- if body_msg.shape.type is SolidPrimitive.BOX:
- geom = BoxShape(body_msg.shape.dimensions[SolidPrimitive.BOX_X],
- body_msg.shape.dimensions[SolidPrimitive.BOX_Y],
- body_msg.shape.dimensions[SolidPrimitive.BOX_Z])
- elif body_msg.shape.type is SolidPrimitive.CYLINDER:
- geom = CylinderShape(body_msg.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS],
- body_msg.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT])
- elif body_msg.shape.type is SolidPrimitive.SPHERE:
- geom = SphereShape(body_msg.shape.dimensions[SolidPrimitive.SPHERE_RADIUS])
- else:
- raise CorruptShapeException("Invalid primitive shape '{}' of world body '{}'".format(body_msg.shape.type, body_msg.name))
- elif body_msg.type is WorldBody.URDF_BODY:
- # TODO: complete me
- pass
- else:
- # TODO: replace me by a proper exception that can be reported back to the service client
- raise RuntimeError("Invalid shape of world body: {}".format(body_msg.shape))
- col = CollisionProperty(name=body_msg.name + '_col', geometry=geom)
- vis = VisualProperty(name=body_msg.name + '_vis', geometry=geom)
- return WorldObject(name=body_msg.name, collision_props=[col], visual_props=[vis])

src/giskardpy/plugin_action_server.py

@@ -18,7 +18,7 @@
 from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
 
 from giskardpy.exceptions import MAX_NWSR_REACHEDException, QPSolverException, SolverTimeoutError, InsolvableException, \
- SymengineException, PathCollisionException
+ SymengineException, PathCollisionException, UnknownBodyException
 from giskardpy.plugin import Plugin
 from giskardpy.plugin_log_trajectory import LogTrajectoryPlugin
 from giskardpy.tfwrapper import transform_pose
@@ -225,7 +225,7 @@ def exception_to_error_code(self, exception):
  error_code = MoveResult.MAX_NWSR_REACHED
  elif isinstance(exception, QPSolverException):
  error_code = MoveResult.QP_SOLVER_ERROR
- elif isinstance(exception, KeyError):
+ elif isinstance(exception, UnknownBodyException):
  error_code = MoveResult.UNKNOWN_OBJECT
  elif isinstance(exception, SolverTimeoutError):
  error_code = MoveResult.SOLVER_TIMEOUT

src/giskardpy/plugin_instantaneous_controller.py

@@ -67,8 +67,8 @@ def copy(self):
  def start_always(self):
  super(CartesianBulletControllerPlugin, self).start_always()
  self.next_cmd = {}
+ self.update_controlled_joints_and_links()
  if self.was_urdf_updated():
- self.update_controlled_joints_and_links()
  self.init_controller()
  self.add_js_controller_soft_constraints()
  self.add_collision_avoidance_soft_constraints()