Merge pull request #27 from SemRoCo/slerp

Slerp
SemRoCo · May 6, 2019 · 1446d42 · 1446d42
2 parents 96aab5f + 8bb2bb7
commit 1446d42
Show file tree

Hide file tree

Showing 67 changed files with 5,935 additions and 3,765 deletions.
diff --git a/data/urdfs/ground_plane.urdf b/data/urdfs/ground_plane.urdf
@@ -0,0 +1,28 @@
+<?xml version="1.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -5"/>
+      <geometry>
+        <box size="100 100 10"/>
+      </geometry>
+       <material name="gray">
+        <color rgba="0.8 0.8 0.8 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -5"/>
+      <geometry>
+        <box size="100 100 10"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
diff --git a/data/urdfs/tiny_ball.urdf b/data/urdfs/tiny_ball.urdf
@@ -0,0 +1,28 @@
+<?xml version="1.0" ?>
+<robot name="ball">
+  <link name="ballLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+       <material name="invisible">
+        <color rgba="1 1 1 0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
diff --git a/dependencies.txt b/dependencies.txt
@@ -8,4 +8,4 @@ qpoases>=3.1.0
 giskard_msgs>=0.1.0
 py_trees_ros>=0.5.14
 py_trees>=0.5.12
-pybullet==2.4.2
+pybullet==2.4.8
diff --git a/launch/giskardpy_boxy.launch b/launch/giskardpy_boxy.launch
@@ -20,6 +20,8 @@
     <rosparam param="collision_time_threshold">15</rosparam>
     <rosparam param="debug">False</rosparam>
     <rosparam param="tree_tick_rate">.2</rosparam>
+    <rosparam param="slerp">False</rosparam>
+    <rosparam param="plot_trajectory">False</rosparam>
   </node>
 
   <node pkg="giskardpy" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">

diff --git a/launch/giskardpy_donbot.launch b/launch/giskardpy_donbot.launch
@@ -3,8 +3,12 @@
   <arg name="gui" default="False"/>
 
   <node pkg="giskardpy" type="giskard_trees.py" name="giskard" output="screen">
-    <param name="path_to_data_folder" value="$(find giskardpy)/data/donbot" />
+    <param name="path_to_data_folder" value="$(find giskardpy)/data" />
     <param name="enable_gui" value="$(arg gui)"/>
+    <rosparam param="enable_collision_marker">True</rosparam>
+    <rosparam param="enable_visualization">True</rosparam>
+    <rosparam param="plot_trajectory">False</rosparam>
+    <rosparam param="debug">False</rosparam>
     <rosparam param="map_frame">map</rosparam>
     <rosparam param="joint_convergence_threshold">0.001</rosparam>
     <rosparam param="wiggle_precision_threshold">5</rosparam>
@@ -16,11 +20,9 @@
     <rosparam param="fill_velocity_values">False</rosparam>
     <rosparam param="nWSR">None</rosparam> <!-- None results in a nWSR estimation thats fine most of the time -->
     <rosparam param="root_link">odom</rosparam>
-    <rosparam param="enable_collision_marker">True</rosparam>
     <rosparam param="collision_time_threshold">15</rosparam>
-    <rosparam param="debug">False</rosparam>
     <rosparam param="tree_tick_rate">0.1</rosparam>
-    <rosparam param="enable_visualization">True</rosparam>
+    <rosparam param="slerp">True</rosparam>
   </node>
 
   <node pkg="giskardpy" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">

diff --git a/launch/giskardpy_hsr.launch b/launch/giskardpy_hsr.launch
@@ -21,6 +21,8 @@
     <rosparam param="debug">True</rosparam>
     <rosparam param="tree_tick_rate">1</rosparam>
     <rosparam param="enable_visualization">False</rosparam>
+    <rosparam param="slerp">False</rosparam>
+    <rosparam param="plot_trajectory">False</rosparam>
     <!-- remaps for real hsr -->
     <!-- <remap from="/whole_body_controller/state" to="/hsrb/arm_trajectory_controller/state"/> -->
     <!-- <remap from="/whole_body_controller/follow_joint_trajectory" to="/hsrb/arm_trajectory_controller/follow_joint_trajectory"/> -->

diff --git a/launch/giskardpy_pr2.launch b/launch/giskardpy_pr2.launch
@@ -5,7 +5,11 @@
   <node pkg="giskardpy" type="giskard_trees.py" name="giskard" output="screen">
     <param name="path_to_data_folder" value="$(find giskardpy)/data/pr2" />
     <param name="enable_gui" value="$(arg gui)"/>
+    <rosparam param="debug">False</rosparam>
+    <rosparam param="enable_visualization">False</rosparam>
+    <rosparam param="enable_collision_marker">False</rosparam>
     <rosparam param="map_frame">map</rosparam>
+    <rosparam param="root_link">base_footprint</rosparam>
     <rosparam param="joint_convergence_threshold">0.002</rosparam>
     <rosparam param="wiggle_precision_threshold">7</rosparam>
     <rosparam param="max_traj_length">30</rosparam>
@@ -15,12 +19,10 @@
     <rosparam param="default_collision_avoidance_distance">0.05</rosparam>
     <rosparam param="fill_velocity_values">False</rosparam>
     <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="collision_time_threshold">15</rosparam>
-    <rosparam param="debug">False</rosparam>
     <rosparam param="tree_tick_rate">0.1</rosparam>
-    <rosparam param="enable_visualization">True</rosparam>
+    <rosparam param="slerp">False</rosparam>
+    <rosparam param="plot_trajectory">False</rosparam>
   </node>
 
   <node pkg="giskardpy" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">

diff --git a/launch/test_urdf_objects.launch b/launch/test_urdf_objects.launch
diff --git a/scripts/add_urdf.py b/scripts/add_urdf.py
@@ -44,8 +44,8 @@
                                   js_topic=rospy.get_param('~js', None),
                                   pose=pose)
         if result.error_codes == result.SUCCESS:
-            rospy.loginfo('urdf \'{}\' added'.format(name))
+            rospy.loginfo('urdfs \'{}\' added'.format(name))
         else:
-            rospy.logwarn('failed to add urdf \'{}\''.format(name))
+            rospy.logwarn('failed to add urdfs \'{}\''.format(name))
     except KeyError:
         rospy.loginfo('Example call: rosrun giskardpy add_urdf.py _name:=kitchen _param:=kitchen_description _js:=kitchen_joint_states _root_frame:=world _frame_id:=map')
diff --git a/scripts/giskard_trees.py b/scripts/giskard_trees.py
@@ -1,167 +1,23 @@
 #!/usr/bin/env python
-import functools
-from time import time
-
 import rospy
-from control_msgs.msg import JointTrajectoryControllerState
-from giskard_msgs.msg import MoveAction
-from py_trees import Sequence, Selector, BehaviourTree, Blackboard
-import py_trees
-from py_trees.behaviours import SuccessEveryN, Success
-from py_trees.meta import running_is_failure, success_is_running, failure_is_success, success_is_failure
-
-import giskardpy
-from giskardpy import DEBUG
-from giskardpy.god_map import GodMap
-from giskardpy.identifier import robot_description_identifier, controlled_joints_identifier
-from giskardpy.plugin import PluginBehavior, SuccessPlugin
-from giskardpy.plugin_action_server import GoalReceived, SendResult, GoalCanceled
-from giskardpy.plugin_cleanup import CleanUp
-from giskardpy.plugin_fk import FkPlugin
-from giskardpy.plugin_goal_reached import GoalReachedPlugin
-from giskardpy.plugin_instantaneous_controller import GoalToConstraints, ControllerPlugin
-from giskardpy.plugin_interrupts import CollisionCancel, WiggleCancel
-from giskardpy.plugin_joint_state import JointStatePlugin
-from giskardpy.plugin_kinematic_sim import KinSimPlugin
-from giskardpy.plugin_log_trajectory import LogTrajPlugin
-from giskardpy.plugin_pybullet import PyBulletMonitor, PyBulletUpdatePlugin, CollisionChecker
-from giskardpy.plugin_send_trajectory import SendTrajectory
-from giskardpy.visualization import VisualizationBehavior
-from giskardpy.utils import create_path, resolve_ros_iris_in_urdf, render_dot_tree, check_dependencies
-
+from giskardpy.garden import grow_tree
+from giskardpy.utils import check_dependencies
 
-# TODO add transform3d to package xml
 # TODO add pytest to package xml
-# TODO move to src folder
-
-
-def ini(param_name):
-    # TODO this should be part of sync
-    urdf = rospy.get_param(param_name)
-    urdf = resolve_ros_iris_in_urdf(urdf)
-    Blackboard().god_map.safe_set_data([robot_description_identifier], urdf)
-
-    msg = rospy.wait_for_message(u'/whole_body_controller/state',
-                                 JointTrajectoryControllerState)  # type: JointTrajectoryControllerState
-    Blackboard().god_map.safe_set_data([controlled_joints_identifier], msg.joint_names)
-
-
-def grow_tree():
-    blackboard = Blackboard
-    blackboard.god_map = GodMap()
-
-    gui = rospy.get_param(u'~enable_gui')
-    map_frame = rospy.get_param(u'~map_frame')
-    enable_visualization = rospy.get_param(u'~enable_visualization')
-    debug = rospy.get_param(u'~debug')
-    if debug:
-        giskardpy.PRINT_LEVEL = DEBUG
-    # tree_tick_rate = rospy.get_param(u'~tree_tick_rate')
-    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_joint_weight = rospy.get_param(u'~default_joint_weight')
-    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(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(u'/'):
-        path_to_data_folder += u'/'
-
-    action_server_name = u'giskardpy/command'
-
-    ini(u'robot_description')
-
-    # ----------------------------------------------
-    sync = PluginBehavior(u'sync')
-    sync.add_plugin(u'js', JointStatePlugin())
-    sync.add_plugin(u'pm', PyBulletMonitor(map_frame, root_link, path_to_data_folder, gui))
-    sync.add_plugin(u'fk', FkPlugin())
-    sync.add_plugin(u'in sync', SuccessPlugin())
-    # ----------------------------------------------
-    wait_for_goal = Selector(u'wait for goal')
-    wait_for_goal.add_child(GoalReceived(u'has goal', action_server_name, MoveAction))
-    monitor = PluginBehavior(u'monitor')
-    monitor.add_plugin(u'js', JointStatePlugin())
-    monitor.add_plugin(u'pm', PyBulletMonitor(map_frame, root_link, path_to_data_folder, gui))
-    monitor.add_plugin(u'fk', FkPlugin())
-    monitor.add_plugin(u'pybullet updater', PyBulletUpdatePlugin(path_to_data_folder, gui))
-    wait_for_goal.add_child(monitor)
-    # ----------------------------------------------
-    planning = failure_is_success(Selector)(u'planning')
-    planning.add_child(GoalCanceled(u'goal canceled', action_server_name))
-    planning.add_child(CollisionCancel(u'in collision', collision_time_threshold))
-    planning.add_child(success_is_failure(VisualizationBehavior)(u'visualization', enable_visualization))
-
-    actual_planning = PluginBehavior(u'planning', sleep=0)
-    actual_planning.add_plugin(u'kin sim', KinSimPlugin(sample_period))
-    actual_planning.add_plugin(u'fk', FkPlugin())
-    actual_planning.add_plugin(u'pm', PyBulletMonitor(map_frame, root_link, path_to_data_folder, gui))
-    actual_planning.add_plugin(u'coll', CollisionChecker(default_collision_avoidance_distance, map_frame, root_link,
-                                                         path_to_data_folder, gui))
-    actual_planning.add_plugin(u'controller', ControllerPlugin(path_to_data_folder, default_joint_vel_limit,
-                                                               default_joint_weight, nWSR))
-    actual_planning.add_plugin(u'log', LogTrajPlugin())
-    actual_planning.add_plugin(u'goal reached', GoalReachedPlugin(joint_convergence_threshold))
-    actual_planning.add_plugin(u'wiggle', WiggleCancel(wiggle_precision_threshold))
-    planning.add_child(actual_planning)
-    # ----------------------------------------------
-
-
-    # ----------------------------------------------
-    publish_result = failure_is_success(Selector)(u'move robot')
-    publish_result.add_child(GoalCanceled(u'goal canceled', action_server_name))
-    publish_result.add_child(SendTrajectory(u'send traj', fill_velocity_values))
-    # ----------------------------------------------
-    root = Sequence(u'root')
-    root.add_child(sync)
-    root.add_child(wait_for_goal)
-    root.add_child(GoalToConstraints(u'update constraints', action_server_name, root_link, default_joint_vel_limit,
-                                     default_joint_weight))
-    root.add_child(planning)
-    root.add_child(CleanUp(u'cleanup'))
-    root.add_child(publish_result)
-    root.add_child(SendResult(u'send result', action_server_name, path_to_data_folder))
-
-    tree = BehaviourTree(root)
-
-    if debug:
-        def post_tick(snapshot_visitor, behaviour_tree):
-            print(u'\n' + py_trees.display.ascii_tree(behaviour_tree.root,
-                                                      snapshot_information=snapshot_visitor))
-
-        snapshot_visitor = py_trees.visitors.SnapshotVisitor()
-        tree.add_post_tick_handler(functools.partial(post_tick, snapshot_visitor))
-        tree.visitors.append(snapshot_visitor)
-        path = path_to_data_folder + u'/tree'
-        create_path(path)
-        render_dot_tree(root, name=path)
-
-    blackboard.time = time()
-
-    # TODO fail if monitor is not called once
-    tree.setup(30)
-    return tree
+# TODO add transform3d to package xml
 
 
 if __name__ == u'__main__':
     rospy.init_node(u'giskard')
     check_dependencies()
-    tree_tick_rate = rospy.get_param(u'~tree_tick_rate')
+    tree_tick_rate = 1. / rospy.get_param(u'~tree_tick_rate')
     tree = grow_tree()
+
+    sleeper = rospy.Rate(tree_tick_rate)
     while not rospy.is_shutdown():
         try:
             tree.tick()
-            rospy.sleep(tree_tick_rate)
+            sleeper.sleep()
         except KeyboardInterrupt:
             break
-    print(u'\n')
+    print(u'\n')
diff --git a/scripts/interactive_marker.py b/scripts/interactive_marker.py
@@ -193,7 +193,7 @@ def __call__(self, feedback):
                 self.giskard.set_cart_goal(self.root_link, self.tip_link, p)
 
                 if not self.enable_self_collision:
-                    self.giskard.disable_self_collision()
+                    self.giskard.allow_self_collision()
 
                 self.giskard.plan_and_execute(wait=False)
                 self.pub_goal_marker(feedback.header, feedback.pose)

diff --git a/scripts/joint_goal_publisher.py b/scripts/joint_goal_publisher.py
@@ -55,7 +55,7 @@ def init_collada(self, robot):
 
     def init_urdf(self, robot):
         """
-        reads the controllable joints from a urdf
+        reads the controllable joints from a urdfs
         :param robot:
         """
         robot = robot.getElementsByTagName('robot')[0]
@@ -258,7 +258,7 @@ def send_goal(self):
             goal_dict[key] = value.get()
 
         if self.allow_self_collision.get():
-            jgp.giskard_wrapper.disable_self_collision()
+            jgp.giskard_wrapper.allow_self_collision()
         else:
             jgp.giskard_wrapper.avoid_collision(self.collision_distance, body_b=jgp.giskard_wrapper.robot_name)