robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

add skill launch, refactor pick-place pose loader

Browse source
This commit is contained in:
Ilya Uraev 2023-11-30 13:47:33 +03:00
parent 034e172f62
commit d72c06efea
14 changed files with 1032 additions and 191 deletions
Download patch file Download diff file Expand all files Collapse all files

4
rbs_bringup/CMakeLists.txt
View file

@ -16,6 +16,10 @@ install(
DESTINATION share/${PROJECT_NAME}
)
# Install Python modules
ament_python_install_package(rbs_launch_utils)
ament_python_install_module(rbs_launch_utils/launch_common.py)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights

622
rbs_bringup/config/rbs.rviz Normal file
View file

@ -0,0 +1,622 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1
- /TF1/Frames1
Splitter Ratio: 0.49496981501579285
Tree Height: 1468
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/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
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /display_contacts
Value: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
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
base:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
base_link_inertia:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
box1_place:
Alpha: 1
Show Axes: false
Show Trail: false
box2_place:
Alpha: 1
Show Axes: false
Show Trail: false
box3_place:
Alpha: 1
Show Axes: false
Show Trail: false
box4_place:
Alpha: 1
Show Axes: false
Show Trail: false
box5_place:
Alpha: 1
Show Axes: false
Show Trail: false
box6_place:
Alpha: 1
Show Axes: false
Show Trail: false
flange:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ft_frame:
Alpha: 1
Show Axes: false
Show Trail: false
inner_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
outer_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_inner_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_inner_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_grasp_point:
Alpha: 1
Show Axes: false
Show Trail: false
shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tool0:
Alpha: 1
Show Axes: false
Show Trail: false
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base:
Value: false
base_link:
Value: false
base_link_inertia:
Value: false
box1:
Value: true
box1_place:
Value: true
box2:
Value: true
box2_place:
Value: true
box3:
Value: true
box3_place:
Value: true
box4:
Value: true
box4_place:
Value: true
box5:
Value: true
box5_place:
Value: true
box6:
Value: true
box6_place:
Value: true
flange:
Value: false
forearm_link:
Value: false
fork_gt:
Value: true
ft_frame:
Value: false
inner_rgbd_camera:
Value: false
outer_rgbd_camera:
Value: false
robotiq_85_base_link:
Value: false
robotiq_85_left_finger_link:
Value: false
robotiq_85_left_finger_tip_link:
Value: false
robotiq_85_left_inner_knuckle_link:
Value: false
robotiq_85_left_knuckle_link:
Value: false
robotiq_85_right_finger_link:
Value: false
robotiq_85_right_finger_tip_link:
Value: false
robotiq_85_right_inner_knuckle_link:
Value: false
robotiq_85_right_knuckle_link:
Value: false
robotiq_grasp_point:
Value: true
shoulder_link:
Value: false
tool0:
Value: false
upper_arm_link:
Value: false
world:
Value: false
wrist_1_link:
Value: false
wrist_2_link:
Value: false
wrist_3_link:
Value: false
Marker Scale: 0.4000000059604645
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
world:
base_link:
base:
{}
base_link_inertia:
shoulder_link:
upper_arm_link:
forearm_link:
wrist_1_link:
wrist_2_link:
wrist_3_link:
flange:
tool0:
inner_rgbd_camera:
{}
robotiq_85_base_link:
robotiq_85_left_inner_knuckle_link:
{}
robotiq_85_left_knuckle_link:
robotiq_85_left_finger_link:
robotiq_85_left_finger_tip_link:
{}
robotiq_85_right_inner_knuckle_link:
{}
robotiq_85_right_knuckle_link:
robotiq_85_right_finger_link:
robotiq_85_right_finger_tip_link:
{}
robotiq_grasp_point:
{}
ft_frame:
{}
box1:
{}
box1_place:
{}
box2:
{}
box2_place:
{}
box3:
{}
box3_place:
{}
box4:
{}
box4_place:
{}
box5:
{}
box5_place:
{}
box6:
{}
box6_place:
{}
fork_gt:
{}
outer_rgbd_camera:
{}
Update Interval: 0
Value: true
- Class: moveit_rviz_plugin/Trajectory
Color Enabled: false
Enabled: true
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
base:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
base_link_inertia:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
box1_place:
Alpha: 1
Show Axes: false
Show Trail: false
box2_place:
Alpha: 1
Show Axes: false
Show Trail: false
box3_place:
Alpha: 1
Show Axes: false
Show Trail: false
box4_place:
Alpha: 1
Show Axes: false
Show Trail: false
box5_place:
Alpha: 1
Show Axes: false
Show Trail: false
box6_place:
Alpha: 1
Show Axes: false
Show Trail: false
flange:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ft_frame:
Alpha: 1
Show Axes: false
Show Trail: false
inner_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
outer_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_inner_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_inner_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_grasp_point:
Alpha: 1
Show Axes: false
Show Trail: false
shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tool0:
Alpha: 1
Show Axes: false
Show Trail: false
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: false
Name: Trajectory
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Robot Description: robot_description
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 3x
Trail Step Size: 1
Trajectory Topic: /display_planned_path
Use Sim Time: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 6.619869709014893
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.2489434778690338
Y: -0.013962505385279655
Z: 0.13800443708896637
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.6253980398178101
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 1.2853964567184448
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1872
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 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
Selection:
collapsed: false
Tool Properties:
collapsed: false
Trajectory - Trajectory Slider:
collapsed: false
Views:
collapsed: false
Width: 3076
X: 124
Y: 54

22
rbs_bringup/launch/bringup.launch.py
View file

@ -9,6 +9,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
# from rbs_launch_utils.launch_common import load_yaml_abs
def generate_launch_description():
declared_arguments = []
@ -192,7 +193,7 @@ def generate_launch_description():
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "config", "fork_view.rviz"]
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
)
mujoco_model = PathJoinSubstitution(
@ -311,6 +312,24 @@ def generate_launch_description():
}.items(),
condition=IfCondition(launch_moveit))
skills = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_skill_servers'),
'launch',
'skills.launch.py'
])
]),
launch_arguments={
'robot_description': robot_description_content,
'robot_description_semantic': robot_description_semantic_content,
'robot_description_kinematics': robot_description_kinematics,
'use_sim_time': use_sim_time,
'with_gripper_condition': with_gripper_condition,
'points_params_filepath': "gripperPositions.yaml"
}.items()
)
task_planner = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
@ -343,6 +362,7 @@ def generate_launch_description():
control,
simulation,
moveit,
skills,
task_planner,
perception
]

0
rbs_bringup/rbs_launch_utils/__init__.py Normal file
View file

84
rbs_bringup/rbs_launch_utils/launch_common.py Normal file
View file

@ -0,0 +1,84 @@
#!/usr/bin/env python3
# Copyright (c) 2021 PickNik, Inc.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the {copyright_holder} nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Lovro Ivanov
import math
import os
import yaml
from ament_index_python.packages import get_package_share_directory
def construct_angle_radians(loader, node):
"""Utility function to construct radian values from yaml."""
value = loader.construct_scalar(node)
try:
return float(value)
except SyntaxError:
raise Exception("invalid expression: %s" % value)
def construct_angle_degrees(loader, node):
"""Utility function for converting degrees into radians from yaml."""
return math.radians(construct_angle_radians(loader, node))
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml_abs(absolute_file_path):
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None

18
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -4,7 +4,7 @@
<Sequence>
<Action ID="GetPickPlacePoses"
ObjectName="box1"
GraspDirection = "y"
GraspDirection = "z"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
@ -19,7 +19,6 @@
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
@ -27,7 +26,7 @@
<Action ID="GetPickPlacePoses"
ObjectName="box2"
GraspDirection = "y"
GraspDirection = "z"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
@ -42,7 +41,6 @@
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
@ -50,7 +48,7 @@
<Action ID="GetPickPlacePoses"
ObjectName="box3"
GraspDirection = "y"
GraspDirection = "z"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
@ -65,7 +63,6 @@
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
@ -73,7 +70,7 @@
<Action ID="GetPickPlacePoses"
ObjectName="box4"
GraspDirection = "y"
GraspDirection = "z"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
@ -88,7 +85,6 @@
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
@ -96,7 +92,7 @@
<Action ID="GetPickPlacePoses"
ObjectName="box5"
GraspDirection = "y"
GraspDirection = "z"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
@ -111,7 +107,6 @@
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
@ -119,7 +114,7 @@
<Action ID="GetPickPlacePoses"
ObjectName="box6"
GraspDirection = "y"
GraspDirection = "z"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
@ -134,7 +129,6 @@
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />

51
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -6,48 +6,20 @@ from ament_index_python.packages import get_package_share_directory
import os
import yaml
import math
def construct_angle_radians(loader, node):
"""Utility function to construct radian values from yaml."""
value = loader.construct_scalar(node)
try:
return float(value)
except SyntaxError:
raise Exception("invalid expression: %s" % value)
def construct_angle_degrees(loader, node):
"""Utility function for converting degrees into radians from yaml."""
return math.radians(construct_angle_radians(loader, node))
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
from rbs_launch_utils.launch_common import load_yaml
def generate_launch_description():
bt_config = os.path.join(
get_package_share_directory('rbs_bt_executor'),
'config',
'params.yaml'
)
points = load_yaml(
package_name="rbs_bt_executor",
file_path="config/points.yaml"
)
# bt_config = os.path.join(
# get_package_share_directory('rbs_bt_executor'),
# 'config',
# 'params.yaml'
# )
# points = load_yaml(
# package_name="rbs_bt_executor",
# file_path="config/points.yaml"
# )
gripperPoints = load_yaml(
package_name="rbs_bt_executor",
@ -76,7 +48,6 @@ def generate_launch_description():
return LaunchDescription([
Node(
package='behavior_tree',
namespace='',
executable='bt_engine',
#prefix=['xterm -e gdb -ex run --args'],
parameters=[
@ -88,7 +59,7 @@ def generate_launch_description():
"rbs_add_planning_scene_object",
"rbs_pose_estimation"
]},
gripperPoints,
# gripperPoints,
robot_description_semantic
]
)

33
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

@ -9,9 +9,7 @@ using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
public:
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<GetPickPlacePoseClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
: BtService<GetPickPlacePoseClient>(name, config) {}
GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
@ -31,24 +29,23 @@ public:
BT::NodeStatus handle_response(
GetPickPlacePoseClient::Response::SharedPtr response) override {
RCLCPP_INFO(_node->get_logger(),
"\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
"\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
"orientation.z: %f \n\t orientation.w: %f",
response->grasp[2].position.x, response->grasp[2].position.y,
response->grasp[2].position.z, response->grasp[2].orientation.x,
response->grasp[2].orientation.y,
response->grasp[2].orientation.z,
response->grasp[2].orientation.w);
// RCLCPP_INFO(_node->get_logger(),
// "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
// "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
// "orientation.z: %f \n\t orientation.w: %f",
// response->grasp[1].position.x, response->grasp[1].position.y,
// response->grasp[1].position.z, response->grasp[1].orientation.x,
// response->grasp[1].orientation.y,
// response->grasp[1].orientation.z,
// response->grasp[1].orientation.w);
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[3]);
setOutput<geometry_msgs::msg::Pose>("PostPostGraspPose",
response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[2]);
return BT::NodeStatus::SUCCESS;
}
@ -60,8 +57,8 @@ public:
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>(
"PostPostGraspPose"), // TODO: change to std::vector<>
// TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),

4
rbs_simulation/worlds/mir.sdf
View file

@ -124,10 +124,10 @@
<name>box6</name>
<pose>-0.25 0.75 0.025 0 0 0</pose>
</include>
<include>
<!-- <include>
<uri>model://fork</uri>
<name>fork_gt</name>
<pose>-0.5 0 0.25 0 0 0</pose>
</include>
</include> -->
</world>
</sdf>

9
rbs_skill_servers/CMakeLists.txt
View file

@ -32,6 +32,8 @@ find_package(tf2_eigen REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tinyxml2_vendor REQUIRED)
find_package(TinyXML2 REQUIRED)
find_package (Eigen3 3.3 REQUIRED)
# Default to Fortress
set(SDF_VER 12)
@ -114,7 +116,7 @@ target_compile_definitions(pick_place_pose_loader
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
ament_target_dependencies(gripper_action_server ${deps})
ament_target_dependencies(pick_place_pose_loader ${deps})
ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3)
rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)
@ -155,6 +157,11 @@ install(
DESTINATION include
)
install(
DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
)
install(
TARGETS
move_topose_action_server

0
rbs_bt_executor/config/gripperPositions.yaml → rbs_skill_servers/config/gripperPositions.yaml
View file

44
rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp
View file

@ -1,29 +1,39 @@
#include <rclcpp/rclcpp.hpp>
#include "rclcpp_components/register_node_macro.hpp"
#include <memory>
#include <algorithm>
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include "rclcpp/rclcpp.hpp"
#include "moveit_msgs/msg/grasp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_ros/transform_listener.h>
namespace rbs_skill_actions
{
class GetGraspPickPoseServer : public rclcpp::Node
{
public:
namespace rbs_skill_actions {
class GetGraspPickPoseServer : public rclcpp::Node {
public:
explicit GetGraspPickPoseServer(rclcpp::NodeOptions options);
private:
static std::vector<geometry_msgs::msg::Pose> collect_pose(const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction, const Eigen::Vector3d scale_vec);
private:
static std::vector<geometry_msgs::msg::Pose>
collect_pose(const Eigen::Isometry3d &graspPose,
const geometry_msgs::msg::Vector3 &move_direction,
const Eigen::Vector3d &scale_vec);
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
geometry_msgs::msg::TransformStamped tf_data;
void handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response);
geometry_msgs::msg::TransformStamped place_pose_tf;
geometry_msgs::msg::TransformStamped grasp_pose_tf;
void handle_server(
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
response);
std::vector<double> grasp_param_pose;
Eigen::Affine3d get_Affine_from_arr(const std::vector<double> pose);
};
}
};
} // namespace rbs_skill_actions
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);

117
rbs_skill_servers/launch/skills.launch.py Normal file
View file

@ -0,0 +1,117 @@
import os
from launch import LaunchDescription, LaunchContext
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
ExecuteProcess,
OpaqueFunction
)
from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from rbs_launch_utils.launch_common import load_yaml
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"robot_description",
default_value="",
description="robot description string",
)
)
declared_arguments.append(
DeclareLaunchArgument("robot_description_semantic", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("robot_description_kinematics", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("use_sim_time", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper_condition", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("points_params_filepath", default_value="")
)
robot_description_decl = LaunchConfiguration("robot_description")
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
use_sim_time = LaunchConfiguration("use_sim_time")
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
robot_description = {"robot_description": robot_description_decl}
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
points_params = load_yaml(
"rbs_skill_servers", "config/gripperPositions.yaml"
)
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
parameters= [
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
condition=IfCondition(with_gripper_condition)
)
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader_service_server",
output="screen"
)
nodes_to_start =[
move_topose_action_server,
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
grasp_pose_loader
]
return LaunchDescription(declared_arguments + nodes_to_start)

187
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -1,110 +1,125 @@
//#include "rbs_skill_servers"
#include "rbs_skill_servers/pick_place_pose_loader.hpp"
#include <Eigen/src/Core/Matrix.h>
#include <Eigen/src/Geometry/AngleAxis.h>
#include <Eigen/src/Geometry/Quaternion.h>
#include <Eigen/src/Geometry/Transform.h>
#include <cmath>
#include <rclcpp/logging.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
// #include <Eigen/src/Core/Matrix.h>
// #include <Eigen/src/Geometry/Transform.h>
// #include <rclcpp/logging.hpp>
// #include <tf2/LinearMath/Quaternion.h>
// #include <tf2/exceptions.h>
// #include <tf2/time.h>
// #include <tf2_eigen/tf2_eigen.hpp>
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
using namespace std::chrono_literals;
// rclcpp::Node::SharedPtr g_node = nullptr;
GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
{
tf_buffer_ =
std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
: Node("grasp_place_pose_loader",
options.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true)) {
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
srv_ = create_service<GetGraspPlacePoseService>("/get_pick_place_pose_service",
std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2));
srv_ = create_service<GetGraspPlacePoseService>(
"/get_pick_place_pose_service",
std::bind(&GetGraspPickPoseServer::handle_server, this,
std::placeholders::_1, std::placeholders::_2));
}
void
GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response)
{
std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name
void GetGraspPlacePoseServer::handle_server(
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
response) {
std::string object_name =
request->object_name + "_place"; // TODO: replace with better name
// Load place pose from TF2
try {
tf_data = tf_buffer_->lookupTransform(
"world", rrr.c_str(),
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
tf2::TimePointZero);
} catch (const tf2::TransformException & ex) {
RCLCPP_ERROR(
this->get_logger(), "Could not transform %s to %s: %s",
rrr.c_str(), "world", ex.what());
} catch (const tf2::TransformException &ex) {
RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s",
object_name.c_str(), "world", ex.what());
return;
}
// Load grasp pose from TF2
try {
grasp_pose_tf = tf_buffer_->lookupTransform(
"world", request->object_name.c_str(), tf2::TimePointZero);
} catch (const tf2::TransformException &ex) {
RCLCPP_ERROR(this->get_logger(), "Could not transforms %s to %s: %s",
request->object_name.c_str(), "world", ex.what());
}
RCLCPP_DEBUG(this->get_logger(),
"Grasp pose from tf \n\tx=%f\n\ty=%f\n\tz=%f",
grasp_pose_tf.transform.translation.x,
grasp_pose_tf.transform.translation.y,
grasp_pose_tf.transform.translation.z);
// TODO: Here need to check the parameter
// We can use another parameter library from PickNik
grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array();
RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->grasp_direction.x,
request->grasp_direction.y,
// grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
// .as_double_array();
RCLCPP_DEBUG(this->get_logger(),
"\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->grasp_direction.x, request->grasp_direction.y,
request->grasp_direction.z);
RCLCPP_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->place_direction.x,
request->place_direction.y,
RCLCPP_DEBUG(this->get_logger(),
"\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->place_direction.x, request->place_direction.y,
request->place_direction.z);
Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose);
Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data);
//std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl;
//std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl;
Eigen::Vector3d vec_grasp(0.15,0.15,0.02);
Eigen::Vector3d vec_place(0,0,0.15);
response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp);
response->place = collect_pose(place_pose, request->place_direction, vec_place);
auto grasp_pose = tf2::transformToEigen(grasp_pose_tf);
auto place_pose = tf2::transformToEigen(place_pose_tf);
Eigen::Vector3d scale_grasp(0, 0, 0.10);
Eigen::Vector3d scale_place(0, 0, 0.15);
response->grasp =
collect_pose(grasp_pose, request->grasp_direction, scale_grasp);
response->place =
collect_pose(place_pose, request->place_direction, scale_place);
}
std::vector<geometry_msgs::msg::Pose>
GetGraspPlacePoseServer::collect_pose(
const Eigen::Affine3d pose,
const geometry_msgs::msg::Vector3 direction,
const Eigen::Vector3d scale_vec)
{
std::vector<geometry_msgs::msg::Pose> pose_v_;
pose_v_.push_back(tf2::toMsg(pose));
Eigen::Vector3d posedir;
Eigen::Affine3d pose_ = pose;
tf2::fromMsg(direction, posedir);
Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose();
Ixy.diagonal() = posedir;
Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
Iz.diagonal() = Eigen::Vector3d::UnitZ();
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
const Eigen::Isometry3d &graspPose,
const geometry_msgs::msg::Vector3 &move_direction,
const Eigen::Vector3d &scale_vec) {
std::vector<geometry_msgs::msg::Pose> poses{};
// Add GraspPose as base point
Eigen::Isometry3d grasp_pose{graspPose};
Eigen::Quaterniond theGraspOrientation(grasp_pose.linear());
theGraspOrientation =
theGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
grasp_pose.rotate(theGraspOrientation);
poses.push_back(tf2::toMsg(grasp_pose));
// PreGrasp calculation
Eigen::Vector3d preGraspTranslation =
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
Eigen::Quaterniond preGraspOrientation(graspPose.linear());
preGraspOrientation =
preGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
Eigen::Isometry3d preGraspPose = Eigen::Isometry3d::Identity();
preGraspPose.translate(preGraspTranslation);
preGraspPose.rotate(preGraspOrientation);
poses.push_back(tf2::toMsg(preGraspPose));
if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0]
{
std::cout << "\n TBD" << std::endl;
}
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0]
{
// Gp -- grasp point frame
pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y
pose_v_.push_back(tf2::toMsg(pose_));
pose_.pretranslate(Iz * scale_vec); // Gp-y + z
pose_v_.push_back(tf2::toMsg(pose_));
pose_.pretranslate(Ixy * scale_vec); // Gp+z
pose_v_.push_back(tf2::toMsg(pose_));
}
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1]
{
pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp
pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp
}
else
{
std::cout << "\n TBD" << std::endl;
}
return pose_v_;
}
// PostGrasp calculation
Eigen::Vector3d postGraspTranslation =
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
Eigen::Quaterniond postGraspOrientation(graspPose.linear());
postGraspOrientation =
postGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
Eigen::Affine3d
GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose)
{
Eigen::Vector3d point(std::vector<double>(pose.begin(), pose.begin()+3).data());
Eigen::Quaterniond quat(std::vector<double>(pose.begin()+3, pose.end()).data());
Eigen::Affine3d aff;
aff.translation() = point;
aff.linear() = quat.toRotationMatrix();
return aff;
Eigen::Isometry3d postGraspPose = Eigen::Isometry3d::Identity();
postGraspPose.translate(postGraspTranslation);
postGraspPose.rotate(postGraspOrientation);
poses.push_back(tf2::toMsg(postGraspPose));
return poses;
}