Skip to content
Snippets Groups Projects
Commit 41547442 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

initial test-setup for side-wise grasping

parent 09ca847a
No related branches found
No related tags found
1 merge request!2Feature/public grasp side
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 8
− 0
View file @ 41547442
......@@ -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()
test/grasp_from_side_test.cpp 0 → 100644
+ 159
− 0
View file @ 41547442
#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();
}
test/plan_visualizer.rviz 0 → 100644
+ 286
− 0
View file @ 41547442
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
test/side_grasping.test 0 → 100644
+ 10
− 0
View file @ 41547442
<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>
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment