Merge pull request #161 from SemRoCo/feature_functions

Feature functions
SemRoCo · Aug 22, 2024 · 645df74 · 645df74
2 parents 83b833b + 70867e6
commit 645df74
Show file tree

Hide file tree

Showing 16 changed files with 933 additions and 61 deletions.
diff --git a/.github/workflows/ci_standalone.yml b/.github/workflows/ci_standalone.yml
@@ -55,6 +55,8 @@ jobs:
       test3: test/test_integration_pr2.py::TestWorldManipulation
       test4: test/test_integration_pr2.py::TestManipulability::test_manip1
       test5: test/test_integration_pr2.py::TestWeightScaling
+      test6: test/test_integration_pr2.py::TestFeatureFunctions
+      test7: test/test_integration_pr2.py::TestEndMotionReason
   pr2_part2:
     needs: [ build_dependencies ]
     uses: ./.github/workflows/reusable_robot_ci.yml

diff --git a/launch/giskardpy_pr2_standalone_vrb.launch b/launch/giskardpy_pr2_standalone_vrb.launch
@@ -0,0 +1,7 @@
+<launch>
+
+    <include file="$(find pr2_description)/robots/upload_pr2.launch"/>
+
+    <node pkg="giskardpy" type="pr2_standalone_vrb.py" name="giskard" output="screen"/>
+
+</launch>
diff --git a/launch/giskardpy_stretch_standalone.launch b/launch/giskardpy_stretch_standalone.launch
@@ -1,14 +1,14 @@
 <launch>
 
-    <include file="$(find stretch_description)/launch/upload.launch"/>
+    <param name="robot_description" command="xacro $(find stretch_description)/urdf/stretch_description_dex.xacro" />
 
     <node pkg="giskardpy" type="stretch_standalone.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">
-            - [map, stretch/link_grasp_center]
-            - [map, stretch/base_link]
+            - [map, link_grasp_center]
+            - [map, base_link]
         </rosparam>
     </node>
 

diff --git a/launch/giskardpy_stretch_standalone_vrb.launch b/launch/giskardpy_stretch_standalone_vrb.launch
@@ -0,0 +1,7 @@
+<launch>
+
+    <param name="robot_description" command="xacro $(find stretch_description)/urdf/stretch_description_dex.xacro" />
+
+    <node pkg="giskardpy" type="stretch_standalone_vrb.py" name="giskard" output="screen"/>
+
+</launch>
diff --git a/scripts/iai_robots/pr2/pr2_standalone.py b/scripts/iai_robots/pr2/pr2_standalone.py
@@ -34,6 +34,6 @@
                               drive_joint_name,
                           ]
                       ),
-                      behavior_tree_config=StandAloneBTConfig(publish_tf=True, publish_js=True),
+                      behavior_tree_config=StandAloneBTConfig(publish_tf=True, publish_js=True, debug_mode=True),
                       qp_controller_config=QPControllerConfig())
     giskard.live()
diff --git a/scripts/iai_robots/pr2/pr2_standalone_vrb.py b/scripts/iai_robots/pr2/pr2_standalone_vrb.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+import rospy
+
+from giskardpy.configs.behavior_tree_config import StandAloneBTConfig
+from giskardpy.configs.giskard import Giskard
+from giskardpy.configs.iai_robots.pr2 import PR2CollisionAvoidance, WorldWithPR2Config
+from giskardpy.configs.qp_controller_config import QPControllerConfig, SupportedQPSolver
+from giskardpy.configs.robot_interface_config import StandAloneRobotInterfaceConfig
+
+if __name__ == '__main__':
+    rospy.init_node('giskard')
+    drive_joint_name = 'brumbrum'
+    giskard = Giskard(world_config=WorldWithPR2Config(drive_joint_name=drive_joint_name),
+                      collision_avoidance_config=PR2CollisionAvoidance(drive_joint_name=drive_joint_name),
+                      robot_interface_config=StandAloneRobotInterfaceConfig(
+                          [
+                              'torso_lift_joint',
+                              'head_pan_joint',
+                              'head_tilt_joint',
+                              'r_shoulder_pan_joint',
+                              'r_shoulder_lift_joint',
+                              'r_upper_arm_roll_joint',
+                              'r_forearm_roll_joint',
+                              'r_elbow_flex_joint',
+                              'r_wrist_flex_joint',
+                              'r_wrist_roll_joint',
+                              'l_shoulder_pan_joint',
+                              'l_shoulder_lift_joint',
+                              'l_upper_arm_roll_joint',
+                              'l_forearm_roll_joint',
+                              'l_elbow_flex_joint',
+                              'l_wrist_flex_joint',
+                              'l_wrist_roll_joint',
+                              drive_joint_name,
+                          ]
+                      ),
+                      behavior_tree_config=StandAloneBTConfig(publish_tf=True, publish_js=False, debug_mode=True,
+                                                              simulation_max_hz=20),
+                      qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.qpSWIFT))
+    giskard.live()
diff --git a/scripts/iai_robots/stretch/stretch_standalone.py b/scripts/iai_robots/stretch/stretch_standalone.py
@@ -12,5 +12,5 @@
     giskard = Giskard(world_config=WorldWithDiffDriveRobot(),
                       collision_avoidance_config=StretchCollisionAvoidanceConfig(),
                       robot_interface_config=StretchStandaloneInterface(),
-                      behavior_tree_config=StandAloneBTConfig())
+                      behavior_tree_config=StandAloneBTConfig(publish_tf=True, publish_js=True, debug_mode=True))
     giskard.live()
diff --git a/scripts/iai_robots/stretch/stretch_standalone_vrb.py b/scripts/iai_robots/stretch/stretch_standalone_vrb.py
@@ -0,0 +1,20 @@
+#!/usr/bin/env python
+import rospy
+
+from giskardpy.configs.behavior_tree_config import StandAloneBTConfig
+from giskardpy.configs.giskard import Giskard
+from giskardpy.configs.iai_robots.stretch import StretchCollisionAvoidanceConfig, StretchStandaloneInterface
+from giskardpy.configs.world_config import WorldWithDiffDriveRobot
+from giskardpy.configs.qp_controller_config import QPControllerConfig, SupportedQPSolver
+from giskardpy.model.ros_msg_visualization import VisualizationMode
+
+if __name__ == '__main__':
+    rospy.init_node('giskard')
+    giskard = Giskard(world_config=WorldWithDiffDriveRobot(),
+                      collision_avoidance_config=StretchCollisionAvoidanceConfig(),
+                      robot_interface_config=StretchStandaloneInterface(),
+                      behavior_tree_config=StandAloneBTConfig(publish_js=False, publish_tf=True, simulation_max_hz=20,
+                                                              debug_mode=True,
+                                                              visualization_mode=VisualizationMode.CollisionsDecomposedFrameLocked),
+                      qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.qpSWIFT))
+    giskard.live()