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

change gripper control from moveit to ros2_control

Browse source
This commit is contained in:
Ilya Uraev 2022-03-08 00:42:34 +04:00
parent 27271c0e06
commit b4339edc19
13 changed files with 116 additions and 43 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rasmt_moveit_config/config/ompl_planning.yaml
View file

@ -154,8 +154,7 @@ rasmt_arm_group:
- LazyPRMstar
- SPARS
- SPARStwo
projection_evaluator: joints(rasmt_Rot_Z_1,rasmt_Rot_Y_1)
longest_valid_segment_fraction: 0.005
rasmt_hand_arm_group:
planner_configs:
- AnytimePathShortening

10
rasmt_moveit_config/config/rasmt.srdf
View file

@ -10,15 +10,15 @@
<!--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="rasmt_arm_group">
<chain base_link="rasmt_Base_Link" tip_link="rasmt_tool_end_point"/>
<chain base_link="rasmt_Base_Link" tip_link="rasmt_tool0"/>
</group>
<group name="rasmt_hand_arm_group">
<!--group name="rasmt_hand_arm_group">
<link name="rasmt_Grip_Body"/>
<link name="rasmt_Grip_L"/>
<link name="rasmt_Grip_R"/>
<joint name="rasmt_Slide_1"/>
<joint name="rasmt_Slide_2"/>
</group>
</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 name="home_position" group="rasmt_arm_group">
<joint name="rasmt_Rot_Y_1" value="0"/>
@ -44,7 +44,7 @@
<joint name="rasmt_Rot_Z_2" value="3.1416"/>
<joint name="rasmt_Rot_Z_3" value="0.0521"/>
</group_state>
<group_state name="home_hand" group="rasmt_hand_arm_group">
<!--group_state name="home_hand" group="rasmt_hand_arm_group">
<joint name="rasmt_Slide_1" value="0.02"/>
<joint name="rasmt_Slide_2" value="0.02"/>
</group_state>
@ -55,7 +55,7 @@
<group_state name="full_close" group="rasmt_hand_arm_group">
<joint name="rasmt_Slide_1" value="0.0"/>
<joint name="rasmt_Slide_2" value="0.0"/>
</group_state>
</group_state-->
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<!--end_effector name="rasmt_hand" parent_link="rasmt_Dock_Link" group="rasmt_hand_arm_group" parent_group="rasmt_arm_group"/-->
<!--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. -->

16
rasmt_moveit_config/config/rasmt_moveit.rviz
View file

@ -7,7 +7,7 @@ Panels:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 787
Tree Height: 389
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@ -43,7 +43,7 @@ Visualization Manager:
Reference Frame: <Fixed Frame>
Value: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Enabled: false
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
@ -128,10 +128,10 @@ Visualization Manager:
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
Value: false
- Acceleration_Scaling_Factor: 1
Class: moveit_rviz_plugin/MotionPlanning
Enabled: false
Enabled: true
Move Group Namespace: ""
MoveIt_Allow_Approximate_IK: false
MoveIt_Allow_External_Program: false
@ -334,7 +334,7 @@ Visualization Manager:
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: false
Value: true
Velocity_Scaling_Factor: 1
Enabled: true
Global Options:
@ -409,7 +409,7 @@ Window Geometry:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000025e0000017d0000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
@ -417,5 +417,5 @@ Window Geometry:
Views:
collapsed: true
Width: 1848
X: 520
Y: 200
X: 72
Y: 27

16
rasmt_moveit_config/config/rasmt_moveit_controllers.yaml
View file

@ -1,6 +1,6 @@
controller_names:
- rasmt_arm_controller
- rasmt_hand_controller
# - rasmt_hand_controller
rasmt_arm_controller:
action_ns: follow_joint_trajectory
@ -14,10 +14,10 @@ rasmt_arm_controller:
- rasmt_Rot_Z_3
- rasmt_Rot_Y_4
rasmt_hand_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- rasmt_Slide_1
- rasmt_Slide_2
#rasmt_hand_controller:
# action_ns: follow_joint_trajectory
# type: FollowJointTrajectory
# default: true
# joints:
# - rasmt_Slide_1
# - rasmt_Slide_2

7
rasmt_support/config/rasmt_ros2_controllers.yaml
View file

@ -6,7 +6,7 @@ controller_manager:
type: joint_trajectory_controller/JointTrajectoryController
rasmt_hand_controller:
type: joint_trajectory_controller/JointTrajectoryController
type: effort_controllers/JointGroupEffortController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
@ -33,7 +33,8 @@ rasmt_hand_controller:
- rasmt_Slide_1
- rasmt_Slide_2
command_interfaces:
- position
- effort
state_interfaces:
- position
- velocity
- velocity
- effort

8
rasmt_support/urdf/robot/rasmt_single_macro.xacro
View file

@ -409,13 +409,13 @@
<axis xyz="0 0 0"/>
</joint>
<link name="${prefix}_tool_end_point"/>
<!--link name="${prefix}_tool_end_point"/>
<joint name="${prefix}_tool_end_point_to_tool0" type="fixed">
<parent link="${prefix}_tool0"/>
<parent link="${prefix}_Dock_Link"/>
<child link="${prefix}_tool_end_point"/>
<origin xyz="0 0 ${gripper_length}" rpy="0 0 0"/>
<origin xyz="0 0 ${gripper_length+0.12324}" rpy="0 0 0"/>
<axis xyz="0 0 0"/>
</joint>
</joint-->
</xacro:macro>
</robot>

12
rasmt_support/urdf/tools/rasmt_hand_control.xacro
View file

@ -17,10 +17,10 @@
</xacro:unless>
<joint name="${prefix}_Slide_1">
<command_interface name="position">
<command_interface name="effort">
<!-- better to use radians as min max first -->
<param name="min">-1</param>
<param name="max"> 1</param>
<param name="min">-10</param>
<param name="max">10</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
@ -30,9 +30,9 @@
<joint name="${prefix}_Slide_2">
<!-- <param name="mimic">${prefix}_Slide_1</param>
<param name="multiplier">1</param> -->
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
<command_interface name="effort">
<param name="min">-10</param>
<param name="max">10</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>

10
robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
View file

@ -13,9 +13,13 @@ class MoveToPose : public BtAction<MoveToPoseAction>
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config) {
target_pose_.position.x = 0.1;
target_pose_.position.y = 0.1;
target_pose_.position.z = 0.6;
target_pose_.position.x = 0.2;
target_pose_.position.y = 0.2;
target_pose_.position.z = 0.7;
//target_pose_.orientation.x = 0;
//target_pose_.orientation.y = 0.9764;
//target_pose_.orientation.z = 0;
//target_pose_.orientation.w = 0.2160;
}
MoveToPoseAction::Goal populate_goal() override

3
robossembler_interfaces/CMakeLists.txt
View file

@ -26,7 +26,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/PropertyValuePair.msg"
"msg/ActionFeedbackStatusConstants.msg"
"msg/ActionResultStatusConstants.msg"
DEPENDENCIES geometry_msgs
"srv/GripperCommand.srv"
DEPENDENCIES geometry_msgs std_msgs
)
if(BUILD_TESTING)

3
robossembler_interfaces/srv/GripperCommand.srv Normal file
View file

@ -0,0 +1,3 @@
float64 value
---
string success

17
robossembler_servers/CMakeLists.txt
View file

@ -27,6 +27,7 @@ find_package(moveit_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(robossembler_interfaces REQUIRED)
find_package(rmw REQUIRED)
set(deps
rclcpp
@ -52,6 +53,15 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
add_library(gripper_cmd_srv_lib SHARED src/gripper_cmd_srv.cpp)
target_compile_definitions(gripper_cmd_srv_lib PRIVATE "GRIPPER_CMD_BUILDING_DLL")
ament_target_dependencies(gripper_cmd_srv_lib ${deps})
rclcpp_components_register_node(gripper_cmd_srv_lib
PLUGIN "GripperCmd"
EXECUTABLE gripper_cmd_srv)
add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
# target_include_directories(move_to_joint_states_action_server PRIVATE include)
ament_target_dependencies(move_to_joint_states_action_server ${deps})
@ -60,11 +70,16 @@ add_executable(move_topose_action_server src/move_topose_action_server.cpp)
# target_include_directories(move_topose_action_server PRIVATE include)
ament_target_dependencies(move_topose_action_server ${deps})
install(
TARGETS
move_topose_action_server
move_to_joint_states_action_server
DESTINATION lib/${PROJECT_NAME}
gripper_cmd_srv_lib
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
ament_package()

46
robossembler_servers/src/gripper_cmd_srv.cpp Normal file
View file

@ -0,0 +1,46 @@
#include <memory>
#include <string>
#include <vector>
#include <cinttypes>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "robossembler_interfaces/srv/gripper_command.hpp"
using GripperCmdSrv = robossembler_interfaces::srv::GripperCommand;
class GripperCmd : public rclcpp::Node
{
public:
explicit GripperCmd(const rclcpp::NodeOptions & options)
: Node("gripper_cmd", options)
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
auto handle_gripper_cmd =
[this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<GripperCmdSrv::Request> request,
std::shared_ptr<GripperCmdSrv::Response> response) -> void
{
RCLCPP_INFO(this->get_logger(), "Service got request value [%f]", request->value);
(void)request_header;
RCLCPP_INFO(this->get_logger(), "request header");
//commands = request->value;
std_msgs::msg::Float64MultiArray msg;
RCLCPP_INFO(this->get_logger(), "init msg");
msg.data[0] = request->value;
RCLCPP_INFO(this->get_logger(), "write to msg");
RCLCPP_INFO(this->get_logger(), "Publishing message");
pub_->publish(msg);
response->success = "SUCCESS";
};
srv_ = create_service<GripperCmdSrv>("/gripper_cmd", handle_gripper_cmd);
pub_ = create_publisher<std_msgs::msg::Float64MultiArray>("/rasmt_hand_controller/commands", 10);
}
private:
rclcpp::Service<GripperCmdSrv>::SharedPtr srv_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_;
}; // class GripperCmd
RCLCPP_COMPONENTS_REGISTER_NODE(GripperCmd)

8
robossembler_servers/src/move_topose_action_server.cpp
View file

@ -18,6 +18,7 @@
// moveit libs
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
#include "moveit/robot_model_loader/robot_model_loader.h"
/*
#include <tf2/LinearMath/Quaternion.h>
@ -109,9 +110,12 @@ private:
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
std::copy(move_group_interface.getJointModelGroupNames().begin(), move_group_interface.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
move_group_interface.setStartState(*move_group_interface.getCurrentState());
move_group_interface.setPoseTarget(goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
@ -121,7 +125,7 @@ private:
if(success)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(plan);
//move_group_interface.execute(plan);
move_group_interface.move();
}else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");