task not working
This commit is contained in:
parent
f4313063e5
commit
cba4b0ed69
@ -30,11 +30,12 @@ find_package(geometry_msgs REQUIRED)
|
||||
find_package(franka_cps_msgs REQUIRED)
|
||||
find_package(franka_msgs REQUIRED)
|
||||
|
||||
find_package(tf2)
|
||||
find_package(tf2_ros)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
|
||||
add_executable(franka_iml_interface src/moveit_interface.cpp)
|
||||
add_executable(franka_grasp_interface src/grasp_interface.cpp)
|
||||
add_executable(franka_pose_recorder src/pose_record.cpp)
|
||||
|
||||
ament_target_dependencies(
|
||||
franka_iml_interface
|
||||
@ -66,12 +67,25 @@ ament_target_dependencies(
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
ament_target_dependencies(
|
||||
franka_pose_recorder
|
||||
rclcpp
|
||||
geometry_msgs
|
||||
tf2
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS franka_iml_interface
|
||||
TARGETS franka_grasp_interface
|
||||
TARGETS franka_pose_recorder
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY
|
||||
launch
|
||||
DESTINATION share/${PROJECT_NAME}/)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
|
||||
120
config/panda.srdf
Normal file
120
config/panda.srdf
Normal file
@ -0,0 +1,120 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from panda_arm_hand.srdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<!--This does not replace URDF, and is not an extension of URDF.
|
||||
This is a format for representing semantic information about the robot structure.
|
||||
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
|
||||
-->
|
||||
<robot name="panda">
|
||||
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
||||
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
|
||||
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
||||
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
|
||||
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
|
||||
<group name="panda_arm">
|
||||
<chain base_link="panda_link0" tip_link="panda_link8"/>
|
||||
</group>
|
||||
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
|
||||
<group_state group="panda_arm" name="ready">
|
||||
<joint name="panda_joint1" value="0"/>
|
||||
<joint name="panda_joint2" value="-0.785"/>
|
||||
<joint name="panda_joint3" value="0"/>
|
||||
<joint name="panda_joint4" value="-2.356"/>
|
||||
<joint name="panda_joint5" value="0"/>
|
||||
<joint name="panda_joint6" value="1.571"/>
|
||||
<joint name="panda_joint7" value="0.785"/>
|
||||
</group_state>
|
||||
<group_state group="panda_arm" name="extended">
|
||||
<joint name="panda_joint1" value="0"/>
|
||||
<joint name="panda_joint2" value="0"/>
|
||||
<joint name="panda_joint3" value="0"/>
|
||||
<joint name="panda_joint4" value="0"/>
|
||||
<joint name="panda_joint5" value="0"/>
|
||||
<joint name="panda_joint6" value="1.571"/>
|
||||
<joint name="panda_joint7" value="0.785"/>
|
||||
</group_state>
|
||||
<group_state group="panda_arm" name="transport">
|
||||
<joint name="panda_joint1" value="0"/>
|
||||
<joint name="panda_joint2" value="-0.5599"/>
|
||||
<joint name="panda_joint3" value="0"/>
|
||||
<joint name="panda_joint4" value="-2.97"/>
|
||||
<joint name="panda_joint5" value="0"/>
|
||||
<joint name="panda_joint6" value="0"/>
|
||||
<joint name="panda_joint7" value="0.785"/>
|
||||
</group_state>
|
||||
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
|
||||
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
|
||||
<virtual_joint child_link="panda_link0" name="virtual_joint" parent_frame="world" type="floating"/>
|
||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||
<disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
|
||||
<disable_collisions link1="panda_link0" link2="panda_link3" reason="Never"/>
|
||||
<disable_collisions link1="panda_link0" link2="panda_link4" reason="Never"/>
|
||||
<disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_link1" link2="panda_link3" reason="Never"/>
|
||||
<disable_collisions link1="panda_link1" link2="panda_link4" reason="Never"/>
|
||||
<disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_link2" link2="panda_link4" reason="Never"/>
|
||||
<disable_collisions link1="panda_link2" link2="panda_link6" reason="Never"/>
|
||||
<disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_link3" link2="panda_link5" reason="Never"/>
|
||||
<disable_collisions link1="panda_link3" link2="panda_link6" reason="Never"/>
|
||||
<disable_collisions link1="panda_link3" link2="panda_link7" reason="Never"/>
|
||||
<disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_link4" link2="panda_link6" reason="Never"/>
|
||||
<disable_collisions link1="panda_link4" link2="panda_link7" reason="Never"/>
|
||||
<disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent"/>
|
||||
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
||||
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
|
||||
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
||||
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
|
||||
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
|
||||
<group name="hand">
|
||||
<link name="panda_hand"/>
|
||||
<link name="panda_leftfinger"/>
|
||||
<link name="panda_rightfinger"/>
|
||||
<joint name="panda_finger_joint1"/>
|
||||
<passive_joint name="panda_finger_joint2"/>
|
||||
</group>
|
||||
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
|
||||
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
|
||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||
<disable_collisions link1="panda_hand" link2="panda_leftfinger" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_hand" link2="panda_rightfinger" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Default"/>
|
||||
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
||||
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
|
||||
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
||||
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
|
||||
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
|
||||
<group name="panda_arm_hand">
|
||||
<group name="panda_arm"/>
|
||||
<group name="hand"/>
|
||||
</group>
|
||||
<group_state group="hand" name="open">
|
||||
<joint name="panda_finger_joint1" value="0.035"/>
|
||||
<joint name="panda_finger_joint2" value="0.035"/>
|
||||
</group_state>
|
||||
<group_state group="hand" name="close">
|
||||
<joint name="panda_finger_joint1" value="0"/>
|
||||
<joint name="panda_finger_joint2" value="0"/>
|
||||
</group_state>
|
||||
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
|
||||
<end_effector group="hand" name="hand" parent_group="panda_arm" parent_link="panda_link8"/>
|
||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||
<disable_collisions link1="panda_hand" link2="panda_link3" reason="Never"/>
|
||||
<disable_collisions link1="panda_hand" link2="panda_link4" reason="Never"/>
|
||||
<disable_collisions link1="panda_hand" link2="panda_link6" reason="Never"/>
|
||||
<disable_collisions link1="panda_hand" link2="panda_link7" reason="Adjacent"/>
|
||||
<disable_collisions link1="panda_leftfinger" link2="panda_link3" reason="Never"/>
|
||||
<disable_collisions link1="panda_leftfinger" link2="panda_link4" reason="Never"/>
|
||||
<disable_collisions link1="panda_leftfinger" link2="panda_link6" reason="Never"/>
|
||||
<disable_collisions link1="panda_leftfinger" link2="panda_link7" reason="Never"/>
|
||||
<disable_collisions link1="panda_link3" link2="panda_rightfinger" reason="Never"/>
|
||||
<disable_collisions link1="panda_link4" link2="panda_rightfinger" reason="Never"/>
|
||||
<disable_collisions link1="panda_link6" link2="panda_rightfinger" reason="Never"/>
|
||||
<disable_collisions link1="panda_link7" link2="panda_rightfinger" reason="Never"/>
|
||||
</robot>
|
||||
144
launch/pose_recoder.launch.py
Executable file
144
launch/pose_recoder.launch.py
Executable file
@ -0,0 +1,144 @@
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription,
|
||||
Shutdown)
|
||||
from launch.conditions import 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
|
||||
import yaml
|
||||
|
||||
|
||||
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:
|
||||
with open(absolute_file_path, 'r') as file:
|
||||
return yaml.safe_load(file)
|
||||
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
||||
return None
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
robot_ip_parameter_name = 'robot_ip'
|
||||
use_fake_hardware_parameter_name = 'use_fake_hardware'
|
||||
fake_sensor_commands_parameter_name = 'fake_sensor_commands'
|
||||
|
||||
robot_ip = LaunchConfiguration(robot_ip_parameter_name)
|
||||
use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
|
||||
fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
|
||||
|
||||
# planning_context
|
||||
franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
|
||||
'panda_arm.urdf.xacro')
|
||||
robot_description_config = Command(
|
||||
[FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true',
|
||||
' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
|
||||
' fake_sensor_commands:=', fake_sensor_commands])
|
||||
|
||||
robot_description = {'robot_description': robot_description_config}
|
||||
|
||||
franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'),
|
||||
'srdf',
|
||||
'panda_arm.srdf.xacro')
|
||||
# Publish TF
|
||||
robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='both',
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
ros2_controllers_path = os.path.join(
|
||||
get_package_share_directory('franka_moveit_config'),
|
||||
'config',
|
||||
'panda_ros_controllers.yaml',
|
||||
)
|
||||
ros2_control_node = Node(
|
||||
package='controller_manager',
|
||||
executable='ros2_control_node',
|
||||
parameters=[robot_description, ros2_controllers_path],
|
||||
remappings=[('joint_states', 'franka/joint_states')],
|
||||
output={
|
||||
'stdout': 'screen',
|
||||
'stderr': 'screen',
|
||||
},
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
|
||||
load_controllers = []
|
||||
for controller in ['joint_state_broadcaster']:
|
||||
load_controllers += [
|
||||
ExecuteProcess(
|
||||
cmd=['ros2 run controller_manager spawner {}'.format(controller)],
|
||||
shell=True,
|
||||
output='screen',
|
||||
)
|
||||
]
|
||||
|
||||
joint_state_publisher = Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher',
|
||||
parameters=[
|
||||
{'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}],
|
||||
)
|
||||
|
||||
franka_robot_state_broadcaster = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['franka_robot_state_broadcaster'],
|
||||
output='screen',
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
|
||||
franka_pose_recorder =Node(
|
||||
package='franka_moveit_interface',
|
||||
executable='franka_pose_recorder',
|
||||
name='franka_pose_recorder',
|
||||
output='screen',
|
||||
parameters=[robot_description]
|
||||
)
|
||||
|
||||
|
||||
robot_arg = DeclareLaunchArgument(
|
||||
robot_ip_parameter_name,
|
||||
default_value='192.168.1.211',
|
||||
description='Hostname or IP address of the robot.')
|
||||
|
||||
use_fake_hardware_arg = DeclareLaunchArgument(
|
||||
use_fake_hardware_parameter_name,
|
||||
default_value='false',
|
||||
description='Use fake hardware')
|
||||
fake_sensor_commands_arg = DeclareLaunchArgument(
|
||||
fake_sensor_commands_parameter_name,
|
||||
default_value='false',
|
||||
description="Fake sensor commands. Only valid when '{}' is true".format(
|
||||
use_fake_hardware_parameter_name))
|
||||
|
||||
gripper_launch_file = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([PathJoinSubstitution(
|
||||
[FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]),
|
||||
launch_arguments={'robot_ip': robot_ip,
|
||||
use_fake_hardware_parameter_name: use_fake_hardware}.items(),
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
robot_arg,
|
||||
use_fake_hardware_arg,
|
||||
fake_sensor_commands_arg,
|
||||
robot_state_publisher,
|
||||
joint_state_publisher,
|
||||
franka_pose_recorder,
|
||||
ros2_control_node,
|
||||
franka_robot_state_broadcaster
|
||||
] + load_controllers
|
||||
)
|
||||
0
launch/run_grasp.launch.py
Normal file
0
launch/run_grasp.launch.py
Normal file
@ -9,8 +9,6 @@
|
||||
|
||||
#include <franka_cps_msgs/action/grasp_sequence.hpp>
|
||||
#include <geometry_msgs/msg/pose.hpp>
|
||||
#include <franka_msgs/action/move.hpp>
|
||||
#include <franka_msgs/action/grasp.hpp>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
@ -18,18 +16,6 @@
|
||||
|
||||
#include <moveit/task_constructor/solvers.h>
|
||||
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#else
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#endif
|
||||
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
|
||||
#include <tf2_eigen/tf2_eigen.hpp>
|
||||
#else
|
||||
#include <tf2_eigen/tf2_eigen.h>
|
||||
#endif
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("franka_grasp_interface");
|
||||
namespace mtc = moveit::task_constructor;
|
||||
@ -39,11 +25,9 @@ class GraspInterfaceNode : public rclcpp::Node
|
||||
public:
|
||||
using GraspSequence = franka_cps_msgs::action::GraspSequence;
|
||||
using GoalHandleGraspSequence = rclcpp_action::ServerGoalHandle<GraspSequence>;
|
||||
using FrankaMove = franka_msgs::action::Move;
|
||||
using FrankaGrasp = franka_msgs::action::Grasp;
|
||||
|
||||
GraspInterfaceNode()
|
||||
: Node("grasp_interface"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_)
|
||||
: Node("grasp_interface")
|
||||
{
|
||||
// Action server for getting new Grasp Sequences
|
||||
action_server_ = rclcpp_action::create_server<GraspSequence>(
|
||||
@ -53,60 +37,26 @@ class GraspInterfaceNode : public rclcpp::Node
|
||||
std::bind(&GraspInterfaceNode::handle_cancel, this, std::placeholders::_1),
|
||||
std::bind(&GraspInterfaceNode::handle_accepted, this, std::placeholders::_1));
|
||||
|
||||
// Publisher for pose
|
||||
pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("franka_grasp_moveit/current_pose", 10);
|
||||
|
||||
// Service for IK
|
||||
|
||||
|
||||
// TF2
|
||||
tf2_ros::Buffer tf_buffer_(this->get_clock());
|
||||
tf2_ros::TransformListener tf_listener_(tf_buffer_);
|
||||
tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&GraspInterfaceNode::compute_endeffector_pose, this));
|
||||
|
||||
// Setup for move groups
|
||||
static const std::string arm_group_name = "panda_arm";
|
||||
static const std::string hand_group_name = "hand";
|
||||
static const std::string hand_frame = "panda_hand";
|
||||
static const std::string hand_group_name = "hand";
|
||||
static const std::string hand_frame = "panda_link8";
|
||||
static const std::string eef_name = "hand";
|
||||
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp_action::Server<GraspSequence>::SharedPtr action_server_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
rclcpp::TimerBase::SharedPtr tf2_timer_;
|
||||
|
||||
void compute_endeffector_pose()
|
||||
{
|
||||
try
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped transform_stamped;
|
||||
transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0));
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
current_pose.header.frame_id = "panda_link8";
|
||||
current_pose.header.stamp = transform_stamped.header.stamp;
|
||||
//Position
|
||||
current_pose.pose.position.x = transform_stamped.transform.translation.x;
|
||||
current_pose.pose.position.y = transform_stamped.transform.translation.y;
|
||||
current_pose.pose.position.z = transform_stamped.transform.translation.z;
|
||||
// Orientation
|
||||
current_pose.pose.orientation.x = transform_stamped.transform.rotation.x;
|
||||
current_pose.pose.orientation.y = transform_stamped.transform.rotation.y;
|
||||
current_pose.pose.orientation.z = transform_stamped.transform.rotation.z;
|
||||
current_pose.pose.orientation.w = transform_stamped.transform.rotation.w;
|
||||
pose_publisher_->publish(current_pose);
|
||||
}
|
||||
catch (tf2::TransformException &ex)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "%s", ex.what());
|
||||
}
|
||||
}
|
||||
static const std::string arm_group_name;
|
||||
static const std::string hand_group_name;
|
||||
static const std::string hand_frame;
|
||||
static const std::string eef_name;
|
||||
const std::array<double, 6> tcp_frame = {0, 0, 0.1034, 0, 0, 0};
|
||||
|
||||
rclcpp_action::GoalResponse handle_goal(
|
||||
const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const GraspSequence::Goal> goal)
|
||||
std::shared_ptr<const GraspSequence::Goal> /*goal*/)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Received goal request!");
|
||||
(void)uuid;
|
||||
@ -157,68 +107,152 @@ class GraspInterfaceNode : public rclcpp::Node
|
||||
task.loadRobotModel(this->shared_from_this());
|
||||
|
||||
// set Planners
|
||||
auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(this->shared_from_this());
|
||||
auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();
|
||||
auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
|
||||
cartesian_planner->setMaxVelocityScalingFactor(1.0);
|
||||
cartesian_planner->setMaxAccelerationScalingFactor(1.0);
|
||||
cartesian_planner->setStepSize(.01);
|
||||
|
||||
std::vector<double> joint_positions;
|
||||
/*
|
||||
// Compute IK for each pose and create corresponding stages
|
||||
if (computeIK(goal->hover, joint_positions)) {
|
||||
// Create and add MoveTo stage using computed joint positions
|
||||
auto move_to_hover = std::make_unique<mtc::stages::MoveTo>("Move to Hover", interpolation_planner);
|
||||
move_to_hover->setGroup(arm_group_name); // set group
|
||||
move_to_hover->setGoal(joint_positions); // set goal
|
||||
task.add(std::move(move_to_hover)); // add Task
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
|
||||
}
|
||||
// Set task properties
|
||||
task.setProperty("group", arm_group_name);
|
||||
task.setProperty("eef", eef_name);
|
||||
task.setProperty("hand", hand_group_name);
|
||||
task.setProperty("hand_grasping_frame", hand_frame);
|
||||
task.setProperty("ik_frame", hand_frame);
|
||||
|
||||
if (computeIK(goal->grasp, joint_positions)) {
|
||||
auto move_to_hover = std::make_unique<mtc::stages::MoveTo>("Move to Hover", interpolation_planner);
|
||||
move_to_hover->setGroup(arm_group_name); // set group
|
||||
move_to_hover->setGoal(joint_positions); // set goal
|
||||
task.add(std::move(move_to_hover)); // add Task
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
|
||||
}
|
||||
// Current State
|
||||
auto current_state = std::make_unique<moveit::task_constructor::stages::CurrentState>("current state");
|
||||
|
||||
if (computeIK(goal->hover, joint_positions)) {
|
||||
// Create and add MoveTo stage using computed joint positions
|
||||
auto move_to_hover2 = std::make_unique<mtc::stages::MoveTo>("Move to Hover", interpolation_planner);
|
||||
move_to_hover2->setGroup(arm_group_name); // set group
|
||||
move_to_hover2->setGoal(joint_positions); // set goal
|
||||
task.add(std::move(move_to_hover2)); // add Task
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
|
||||
}
|
||||
// Open Hand
|
||||
moveit::task_constructor::Stage* initial_state_ptr = nullptr;
|
||||
|
||||
if (computeIK(goal->drop, joint_positions)) {
|
||||
// Create and add MoveTo stage using computed joint positions
|
||||
auto move_to_drop = std::make_unique<mtc::stages::MoveTo>("Move to drop", interpolation_planner);
|
||||
move_to_drop->setGroup(arm_group_name); // set group
|
||||
move_to_drop->setGoal(joint_positions); // set goal
|
||||
task.add(std::move(move_to_drop)); // add Task
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for drop pose");
|
||||
}
|
||||
auto stage1 = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner);
|
||||
stage1.setGroup(hand_group_name);
|
||||
stage1.setGoal("open");
|
||||
initial_state_ptr = stage1.get();
|
||||
task.add(std::move(stage1));
|
||||
|
||||
if (computeIK(goal->idle, joint_positions)) {
|
||||
// Create and add MoveTo stage using computed joint positions
|
||||
auto move_to_idle = std::make_unique<mtc::stages::MoveTo>("Move to idle", interpolation_planner);
|
||||
move_to_idle->setGroup(arm_group_name); // set group
|
||||
move_to_idle->setGoal(joint_positions); // set goal
|
||||
task.add(std::move(move_to_idle)); // add Task
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for idle pose");
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
// Grasping move
|
||||
auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
|
||||
task.properties().exposeTo(grasp.properties(), { "eef", "hand", "group", "ik_frame" });
|
||||
grasp.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
|
||||
|
||||
// Move Hower
|
||||
auto stage2 = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate hower pose");
|
||||
stage2.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
|
||||
stage2.properties().set("marker_ns", "hower_pose");
|
||||
stage2.setPose(goal.hower)
|
||||
stage2.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
|
||||
|
||||
// Compute IK Hower
|
||||
auto wrapper1 = std::make_unique<moveit::task_constructor::stages::ComputeIK>("hower pose IK", std::move(stage));
|
||||
wrapper1.setMaxIKSolutions(2);
|
||||
wrapper1.setMinSolutionDistance(1.0);
|
||||
wrapper1.setIKFrame(vectorToEigen(tcp_frame), hand_frame);
|
||||
wrapper1.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
|
||||
wrapper1.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
|
||||
grasp.insert(std::move(wrapper1));
|
||||
|
||||
// Move Grasp
|
||||
auto stage3 = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
|
||||
stage3.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
|
||||
stage3.properties().set("marker_ns", "grasp_pose");
|
||||
stage3.setPose(goal.grasp)
|
||||
stage3.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
|
||||
|
||||
// Compute IK Grasp
|
||||
auto wrapper2 = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
|
||||
wrapper2.setMaxIKSolutions(2);
|
||||
wrapper2.setMinSolutionDistance(1.0);
|
||||
wrapper2.setIKFrame(vectorToEigen(tcp_frame), hand_frame);
|
||||
wrapper2.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
|
||||
wrapper2.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
|
||||
grasp.insert(std::move(wrapper2));
|
||||
|
||||
// Allow Collision
|
||||
auto stage4 = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
|
||||
stage4.allowCollisions(
|
||||
"object",
|
||||
task.getRobotModel().getJointModelGroup(hand_group_name).getLinkModelNamesWithCollisionGeometry(),
|
||||
true);
|
||||
grasp.insert(std::move(stage4));
|
||||
|
||||
// Close Hand
|
||||
auto stage5 = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner);
|
||||
stage5.setGroup(hand_group_name);
|
||||
stage5.setGoal("close");
|
||||
grasp.insert(std::move(stage5));
|
||||
|
||||
// Move Back to Hower
|
||||
auto stage6 = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate hower pose");
|
||||
stage6.properties().configureInitFrom(Stage::PARENT);
|
||||
stage6.properties().set("marker_ns", "hower_pose");
|
||||
stage6.setPose(goal.hower)
|
||||
stage6.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
|
||||
|
||||
// Compute IK Hower
|
||||
grasp.insert(std::move(wrapper2));
|
||||
|
||||
// Transition to Drop Phase
|
||||
task.add(std::move(grasp));
|
||||
|
||||
|
||||
// Move Drop
|
||||
auto stage7 = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate hower pose");
|
||||
stage7.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
|
||||
stage7.properties().set("marker_ns", "hower_pose");
|
||||
stage7.setPose(goal.drop)
|
||||
stage7.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
|
||||
|
||||
// Compute IK Drop
|
||||
auto wrapper3 = std::make_unique<moveit::task_constructor::stages::ComputeIK>("drop pose IK", std::move(stage));
|
||||
wrapper3.setMaxIKSolutions(2);
|
||||
wrapper3.setMinSolutionDistance(1.0);
|
||||
wrapper3.setIKFrame(vectorToEigen(tcp_frame), hand_frame);
|
||||
wrapper3.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
|
||||
wrapper3.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
|
||||
task.add(std::move(wrapper3));
|
||||
|
||||
// Open Hand
|
||||
auto stage8 = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner);
|
||||
stage8.setGroup(hand_group_name);
|
||||
stage8.setGoal("open");
|
||||
task.add(std::move(stage8));
|
||||
|
||||
// Move Idle
|
||||
auto stage9 = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate idle pose");
|
||||
stage9.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
|
||||
stage9.properties().set("marker_ns", "hower_pose");
|
||||
stage9.setPose(goal.idle)
|
||||
stage9.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
|
||||
|
||||
// Compute IK Idle
|
||||
auto wrapper4 = std::make_unique<moveit::task_constructor::stages::ComputeIK>("drop pose IK", std::move(stage));
|
||||
wrapper4.setMaxIKSolutions(2);
|
||||
wrapper4.setMinSolutionDistance(1.0);
|
||||
wrapper4.setIKFrame(vectorToEigen(tcp_frame), hand_frame);
|
||||
wrapper4.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
|
||||
wrapper4.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
|
||||
task.add(std::move(wrapper4));
|
||||
|
||||
return task;
|
||||
}
|
||||
|
||||
bool executeTask(moveit::task_constructor::Task& task) {
|
||||
// Logic to execute the given task
|
||||
// Return true if successful, false otherwise
|
||||
return 1;
|
||||
bool executeTask(moveit::task_constructor::Task::Task task) {
|
||||
RCLCPP_INFO(LOGGER, "Executing solution trajectory");
|
||||
moveit_msgs::msg::MoveItErrorCodes execute_result;
|
||||
|
||||
execute_result = task.execute(*task.solutions().front());
|
||||
|
||||
if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
|
||||
RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -22,6 +22,7 @@
|
||||
#include "moveit/planning_scene_interface/planning_scene_interface.h"
|
||||
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include <tf2_ros/buffer.h>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
|
||||
77
src/pose_record.cpp
Normal file
77
src/pose_record.cpp
Normal file
@ -0,0 +1,77 @@
|
||||
#include <functional>
|
||||
#include <future>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include "geometry_msgs/msg/transform_stamped.h"
|
||||
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include <tf2_ros/buffer.h>
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("franka_pose_recorder");
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class PoseRecoderNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
using PoseStampedMsg = geometry_msgs::msg::PoseStamped;
|
||||
|
||||
PoseRecoderNode()
|
||||
: Node("pose_recoder"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_)
|
||||
{
|
||||
// Publisher for pose
|
||||
pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("pose_recoder/current_pose", 10);
|
||||
|
||||
|
||||
// TF2
|
||||
tf2_ros::Buffer tf_buffer_{this->get_clock()};
|
||||
tf2_ros::TransformListener tf_listener_{tf_buffer_};
|
||||
tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&PoseRecoderNode::compute_endeffector_pose, this));
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
rclcpp::TimerBase::SharedPtr tf2_timer_;
|
||||
|
||||
void compute_endeffector_pose()
|
||||
{
|
||||
try
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped transform_stamped;
|
||||
transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0));
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
current_pose.header.frame_id = "panda_link8";
|
||||
current_pose.header.stamp = transform_stamped.header.stamp;
|
||||
//Position
|
||||
current_pose.pose.position.x = transform_stamped.transform.translation.x;
|
||||
current_pose.pose.position.y = transform_stamped.transform.translation.y;
|
||||
current_pose.pose.position.z = transform_stamped.transform.translation.z;
|
||||
// Orientation
|
||||
current_pose.pose.orientation.x = transform_stamped.transform.rotation.x;
|
||||
current_pose.pose.orientation.y = transform_stamped.transform.rotation.y;
|
||||
current_pose.pose.orientation.z = transform_stamped.transform.rotation.z;
|
||||
current_pose.pose.orientation.w = transform_stamped.transform.rotation.w;
|
||||
pose_publisher_->publish(current_pose);
|
||||
}
|
||||
catch (tf2::TransformException &ex)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "%s", ex.what());
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<PoseRecoderNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user