bugfix fixed base config & ur5 dualarm config

launch/giskardpy_ur5dualarm_traj.launch

@@ -0,0 +1,13 @@
+<launch>
+
+  <node pkg="giskardpy" type="ur5_dualarm.py" name="giskard" output="screen"/>
+
+  <node pkg="giskardpy" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">
+    <rosparam param="enable_self_collision">False</rosparam>
+    <rosparam param="interactive_marker_chains">
+      - [world, left_ee_link]
+      - [world, right_ee_link]
+    </rosparam>
+  </node>
+
+</launch>

scripts/iai_robots/dualarm/ur5_dualarm.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python3
+import rospy
+
+from giskardpy.configs.giskard import Giskard
+from giskardpy.configs.iai_robots.ur5_dualarm import UR5DualArmWorldConfig, UR5DualArmCollisionAvoidanceConfig, \
+    UR5DualArmJointTrajServerInterface, UR5DualArmVelocityInterface
+from giskardpy.configs.collision_avoidance_config import CollisionAvoidanceConfig
+from giskardpy.configs.behavior_tree_config import ClosedLoopBTConfig
+
+if __name__ == '__main__':
+    rospy.init_node('giskard')
+    giskard = Giskard(world_config=UR5DualArmWorldConfig(),
+                      # collision_avoidance_config=UR5DualArmCollisionAvoidanceConfig(),
+                      robot_interface_config=UR5DualArmVelocityInterface(),
+                      behavior_tree_config=ClosedLoopBTConfig())
+    giskard.live()

src/giskardpy/configs/behavior_tree_config.py

@@ -159,7 +159,8 @@ def add_evaluate_debug_expressions(self):
         else:
             self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=True)
         if god_map.is_open_loop():
-            god_map.tree.execute_traj.prepare_base_control.add_compile_debug_expressions()
+            if not god_map.is_fixed_base_robot:
+                god_map.tree.execute_traj.prepare_base_control.add_compile_debug_expressions()
             god_map.tree.execute_traj.base_closed_loop.add_evaluate_debug_expressions(log_traj=False)
 
     def add_js_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False):

src/giskardpy/configs/iai_robots/ur5_dualarm.py

@@ -0,0 +1,44 @@
+import numpy as np
+
+from giskardpy.configs.collision_avoidance_config import CollisionAvoidanceConfig
+from giskardpy.configs.robot_interface_config import RobotInterfaceConfig, StandAloneRobotInterfaceConfig
+from giskardpy.configs.world_config import WorldWithFixedRobot
+from giskardpy.model.collision_world_syncer import CollisionCheckerLib
+from giskardpy.data_types import Derivatives
+
+
+class UR5DualArmJointTrajServerInterface(RobotInterfaceConfig):
+    def setup(self):
+        self.sync_joint_state_topic('joint_states')
+        self.add_follow_joint_trajectory_server(
+            namespace='/left_arm/pos_joint_traj_controller_left')
+        self.add_follow_joint_trajectory_server(
+            namespace='/right_arm/pos_joint_traj_controller_right')
+
+
+class UR5DualArmVelocityInterface(RobotInterfaceConfig):
+    def setup(self):
+        self.sync_joint_state_topic('joint_states')
+        self.add_joint_velocity_group_controller(namespace='left_arm/joint_group_vel_controller_left')
+        self.add_joint_velocity_group_controller(namespace='right_arm/joint_group_vel_controller_right')
+
+
+class UR5DualArmCollisionAvoidanceConfig(CollisionAvoidanceConfig):
+    def __init__(self):
+        super().__init__()
+
+    def setup(self):
+        self.load_self_collision_matrix('package://giskardpy/self_collision_matrices/iai/ur5dualarm.srdf')
+
+
+class UR5DualArmWorldConfig(WorldWithFixedRobot):
+    def __init__(self):
+        super().__init__({Derivatives.velocity: 0.2,
+                          Derivatives.acceleration: np.inf,
+                          Derivatives.jerk: 15})
+
+    def setup(self):
+        super().setup()
+        self.set_joint_limits(limit_map={Derivatives.velocity: 3,
+                                         Derivatives.jerk: 80},
+                              joint_name='left_wrist_3_joint')

src/giskardpy/configs/world_config.py

@@ -22,6 +22,7 @@ class WorldConfig(ABC):
     def __init__(self):
         god_map.world = WorldTree()
         self.set_default_weights()
+        god_map.is_fixed_base_robot = False
 
     @property
     def world(self) -> WorldTree:
@@ -237,6 +238,7 @@ class WorldWithFixedRobot(WorldConfig):
     def __init__(self, joint_limits: Dict[Derivatives, float] = None):
         super().__init__()
         self._joint_limits = joint_limits
+        god_map.is_fixed_base_robot = True
 
     def setup(self):
         self.set_default_limits(self._joint_limits)

src/giskardpy/god_map.py

@@ -70,6 +70,7 @@ class GodMap:
     ros_visualizer: ROSMsgVisualization
     free_variables: List[FreeVariable]
     controlled_joints: List[Joint]
+    is_fixed_base_robot: bool
 
     def is_goal_msg_type_execute(self):
         return self.move_action_server.goal_msg.type in [MoveGoal.EXECUTE]

src/giskardpy/tree/branches/send_trajectories.py

@@ -19,8 +19,9 @@ def __init__(self, name: str = 'execute traj'):
         super().__init__(name)
         self.move_robots = Parallel(name='move robot', policy=ParallelPolicy.SuccessOnAll(synchronise=True))
         self.add_child(self.move_robots)
-        self.prepare_base_control = PrepareBaseTrajControlLoop()
-        self.insert_child(self.prepare_base_control, 0)
+        if not god_map.is_fixed_base_robot:
+            self.prepare_base_control = PrepareBaseTrajControlLoop()
+            self.insert_child(self.prepare_base_control, 0)
 
         self.base_closed_loop = ControlLoop(log_traj=False, max_hz=god_map.behavior_tree_config.control_loop_max_hz)
         self.base_closed_loop.add_closed_loop_behaviors()