From 415474422bd12e21b287f3be57174bda15dcc7e1 Mon Sep 17 00:00:00 2001
From: SebastianEbert <sebastian.ebert@tu-dresden.de>
Date: Mon, 25 Oct 2021 16:37:47 +0200
Subject: [PATCH] initial test-setup for side-wise grasping
---
CMakeLists.txt | 8 +
test/grasp_from_side_test.cpp | 159 +++++++++++++++++++
test/plan_visualizer.rviz | 286 ++++++++++++++++++++++++++++++++++
test/side_grasping.test | 10 ++
4 files changed, 463 insertions(+)
create mode 100644 test/grasp_from_side_test.cpp
create mode 100644 test/plan_visualizer.rviz
create mode 100644 test/side_grasping.test
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 072f313..7176fa5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -70,4 +70,12 @@ target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUti
add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
+if(CATKIN_ENABLE_TESTING)
+ find_package(rostest REQUIRED)
+ add_rostest_gtest(tests_side_grasping test/side_grasping.test test/grasp_from_side_test.cpp)
+ target_link_libraries(tests_side_grasping ${catkin_LIBRARIES} GraspUtil)
+ add_dependencies(tests_side_grasping panda_grasping_generate_messages_cpp)
+endif()
+
+
diff --git a/test/grasp_from_side_test.cpp b/test/grasp_from_side_test.cpp
new file mode 100644
index 0000000..7b4beee
--- /dev/null
+++ b/test/grasp_from_side_test.cpp
@@ -0,0 +1,159 @@
+#include "ros/ros.h"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Vector3.h"
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <gtest/gtest.h>
+#include <grasp_util.h>
+
+/**
+ * Testing grasping from side.
+ */
+class Grasping : public ::testing::Test {
+protected:
+ void SetUp() override {
+
+ ros::NodeHandle n("panda_grasping");
+ CleanupScene();
+ }
+
+ static void CleanupScene() {
+ moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+
+ for (auto p : planning_scene_interface.getObjects()) {
+ p.second.operation = moveit_msgs::CollisionObject::REMOVE;
+ planning_scene_interface.applyCollisionObject(p.second);
+ }
+ ASSERT_EQ(planning_scene_interface.getObjects().size(), 0);
+ ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0);
+ }
+
+ void doTest(geometry_msgs::Pose &pick_table_pose, geometry_msgs::Pose &place_table_pose, geometry_msgs::Pose obj_pose, geometry_msgs::Pose place_pose){
+
+ moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+ moveit::planning_interface::MoveGroupInterface group("panda_arm");
+
+ std::vector<moveit_msgs::CollisionObject> collision_objects;
+ collision_objects.resize(3);
+
+ collision_objects[0].id = "table1";
+ collision_objects[0].header.frame_id = "panda_link0";
+
+ collision_objects[0].primitives.resize(1);
+ collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
+ collision_objects[0].primitives[0].dimensions.resize(3);
+ collision_objects[0].primitives[0].dimensions[0] = 0.2;
+ collision_objects[0].primitives[0].dimensions[1] = 0.4;
+ collision_objects[0].primitives[0].dimensions[2] = 0.4;
+ collision_objects[0].primitive_poses.resize(1);
+ collision_objects[0].primitive_poses[0] = pick_table_pose;
+ collision_objects[0].operation = collision_objects[0].ADD;
+
+ collision_objects[1].id = "table2";
+ collision_objects[1].header.frame_id = "panda_link0";
+
+ collision_objects[1].primitives.resize(1);
+ collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
+ collision_objects[1].primitives[0].dimensions.resize(3);
+ collision_objects[1].primitives[0].dimensions[0] = 0.4;
+ collision_objects[1].primitives[0].dimensions[1] = 0.2;
+ collision_objects[1].primitives[0].dimensions[2] = 0.4;
+ collision_objects[1].primitive_poses.resize(1);
+ collision_objects[1].primitive_poses[0] = place_table_pose;
+ collision_objects[1].operation = collision_objects[1].ADD;
+
+ collision_objects[2].header.frame_id = "panda_link0";
+ collision_objects[2].id = "object";
+
+ collision_objects[2].primitives.resize(1);
+ collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].CYLINDER;
+ collision_objects[2].primitives[0].dimensions.resize(2);
+ collision_objects[2].primitives[0].dimensions[0] = 0.2;
+ collision_objects[2].primitives[0].dimensions[1] = 0.008;
+ collision_objects[2].primitive_poses.resize(1);
+ collision_objects[2].primitive_poses[0] = obj_pose;
+ collision_objects[2].operation = collision_objects[2].ADD;
+
+ planning_scene_interface.applyCollisionObjects(collision_objects);
+
+ pickAndPlace(obj_pose, planning_scene_interface, group, place_pose);
+ }
+
+ void pickAndPlace(geometry_msgs::Pose &obj_pose, moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
+ moveit::planning_interface::MoveGroupInterface &group, geometry_msgs::Pose &place_pose) {
+
+ tf2::Quaternion gripper_orientation;
+ gripper_orientation.setRPY(0, 0, -M_PI_4);
+
+ geometry_msgs::Vector3 approach_direction;
+ approach_direction.x = 1;
+
+ shape_msgs::SolidPrimitive object;
+ object.type = shape_msgs::SolidPrimitive::CYLINDER;
+ object.dimensions.resize(2);
+ object.dimensions[0] = 0.2;
+ object.dimensions[1] = 0.008;
+
+ GraspUtil grasp_util;
+
+ geometry_msgs::Pose pick_pose = grasp_util.getPickFromMinusXSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+
+ ros::WallDuration(1.0).sleep();
+
+ grasp_util.pickFromSide(group, pick_pose, object, approach_direction,0.06, "table1", "object");
+
+ ros::WallDuration(1.0).sleep();
+
+ grasp_util.placeFromSide(group, place_pose, 0.06, "table2", "object", false);
+ }
+};
+
+TEST_F(Grasping, SIDE_GRASPING_1) {
+
+ geometry_msgs::Pose pick_table_pose;
+ geometry_msgs::Pose place_table_pose;
+ geometry_msgs::Pose obj_pose;
+ geometry_msgs::Pose place_pose;
+
+ pick_table_pose.position.x = 0.5;
+ pick_table_pose.position.y = -0.5;
+ pick_table_pose.position.z = 0.2;
+ pick_table_pose.orientation.w = 1.0;
+
+ place_table_pose.position.x = 0;
+ place_table_pose.position.y = 0.5;
+ place_table_pose.position.z = 0.2;
+ place_table_pose.orientation.w = 1.0;
+
+ obj_pose.position.x = 0.5;
+ obj_pose.position.y = -0.5;
+ obj_pose.position.z = 0.5;
+ obj_pose.orientation.w = 1.0;
+
+ tf2::Quaternion orientation;
+ orientation.setRPY(0, 0, M_PI / 2);
+ place_pose.orientation = tf2::toMsg(orientation);
+ place_pose.position.x = place_table_pose.position.x;
+ place_pose.position.y = place_table_pose.position.y;
+ place_pose.position.z = place_table_pose.position.z + 0.01;
+
+ doTest(pick_table_pose, place_table_pose, obj_pose, place_pose);
+
+}
+
+int main(int argc, char **argv) {
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "grasping_side_test");
+ ros::NodeHandle n("panda_grasping");
+
+ ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
+ ros::Duration(3.0).sleep();
+ ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> STARTING TEST AFTER INIT <<<<<<<<<<<<<<<<");
+
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/plan_visualizer.rviz b/test/plan_visualizer.rviz
new file mode 100644
index 0000000..65ff161
--- /dev/null
+++ b/test/plan_visualizer.rviz
@@ -0,0 +1,286 @@
+Panels:
+ []
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Acceleration_Scaling_Factor: 1
+ Class: moveit_rviz_plugin/MotionPlanning
+ Enabled: true
+ Move Group Namespace: ""
+ MoveIt_Allow_Approximate_IK: false
+ MoveIt_Allow_External_Program: false
+ MoveIt_Allow_Replanning: false
+ MoveIt_Allow_Sensor_Positioning: false
+ MoveIt_Goal_Tolerance: 0
+ MoveIt_Planning_Attempts: 10
+ MoveIt_Planning_Time: 5
+ MoveIt_Use_Cartesian_Path: false
+ MoveIt_Use_Constraint_Aware_IK: true
+ MoveIt_Warehouse_Host: 127.0.0.1
+ MoveIt_Warehouse_Port: 33829
+ MoveIt_Workspace:
+ Center:
+ X: 0
+ Y: 0
+ Z: 0
+ Size:
+ X: 2
+ Y: 2
+ Z: 2
+ Name: MotionPlanning
+ Planned Path:
+ Color Enabled: false
+ Interrupt Display: false
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Loop Animation: true
+ Robot Alpha: 0.5
+ Robot Color: 150; 50; 150
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Show Trail: false
+ State Display Time: 0.05 s
+ Trail Step Size: 1
+ Trajectory Topic: move_group/display_planned_path
+ Planning Metrics:
+ Payload: 1
+ Show Joint Torques: false
+ Show Manipulability: false
+ Show Manipulability Index: false
+ Show Weight Limit: false
+ TextHeight: 0.07999999821186066
+ Planning Request:
+ Colliding Link Color: 255; 0; 0
+ Goal State Alpha: 1
+ Goal State Color: 250; 128; 0
+ Interactive Marker Size: 0
+ Joint Violation Color: 255; 0; 255
+ Planning Group: panda_arm
+ Query Goal State: false
+ Query Start State: false
+ Show Workspace: false
+ Start State Alpha: 1
+ Start State Color: 0; 255; 0
+ Planning Scene Topic: move_group/monitored_planning_scene
+ Robot Description: robot_description
+ Scene Geometry:
+ Scene Alpha: 0.8999999761581421
+ Scene Color: 50; 230; 50
+ Scene Display Time: 0.20000000298023224
+ Show Scene Geometry: true
+ Voxel Coloring: Z-Axis
+ Voxel Rendering: Occupied Voxels
+ Scene Robot:
+ Attached Body Color: 150; 50; 150
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Robot Alpha: 0.5
+ Show Robot Collision: false
+ Show Robot Visual: false
+ Value: true
+ Velocity_Scaling_Factor: 1
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: panda_link0
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz_visual_tools/KeyTool
+ Value: true
+ Views:
+ Current:
+ Class: rviz/XYOrbit
+ Distance: 2.9803097248077393
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 1.0864897966384888
+ Y: 0.3387787342071533
+ Z: 2.2351798634190345e-7
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.270203560590744
+ Target Frame: panda_link0
+ Value: XYOrbit (rviz)
+ Yaw: 0.42677485942840576
+ Saved: ~
+Window Geometry:
+ Height: 1404
+ Hide Left Dock: false
+ Hide Right Dock: false
+ MotionPlanning:
+ collapsed: false
+ MotionPlanning - Trajectory Slider:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd00000002000000000000013b000004e1fc0200000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000080000003640000017200fffffffb0000000800480065006c00700100000334000000e10000000000000000fb0000000a00560069006500770073000000003b000004e1000000000000000000000003000004fe0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000000000004fe0000010000ffffff000004fe000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Width: 1278
+ X: 0
+ Y: 18
\ No newline at end of file
diff --git a/test/side_grasping.test b/test/side_grasping.test
new file mode 100644
index 0000000..3644570
--- /dev/null
+++ b/test/side_grasping.test
@@ -0,0 +1,10 @@
+<launch>
+ <include file="$(find panda_simulation)/launch/simulation.launch"/>
+
+ <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+ args="-d $(find panda_grasping)/test/plan_visualizer.rviz" output="screen">
+ <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+ </node>
+
+ <test test-name="tests_side_grasping" pkg="panda_grasping" type="tests_side_grasping" time-limit="3600.0"/>
+</launch>
--
GitLab