robossembler/cnc-graver-assist
7
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

Pick and place MVP w/o CV

Browse source
This commit is contained in:
Ilya Uraev 2025-03-03 08:38:53 +00:00 • committed by Igor Brylev
parent 2e835aaa74
commit 2763ddbc71
26 changed files with 678 additions and 142 deletions
Download patch file Download diff file Expand all files Collapse all files

6
rbs_mill_assist/CMakeLists.txt
View file

@ -48,9 +48,13 @@ install(
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY launch urdf config world assets meshes
install(DIRECTORY world urdf meshes launch config assets bt/xmls bt/config
DESTINATION share/${PROJECT_NAME})
add_subdirectory(scripts)
add_subdirectory(bt)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights

BIN
rbs_mill_assist/assets/bunker/meshes/bunker.stl Normal file
View file

Binary file not shown.

16
rbs_mill_assist/assets/bunker/model.config Normal file
View file

@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>bunker</name>
<version>0.1</version>
<sdf version='1.10'>model.sdf</sdf>
<author>
<name>Bill Finger</name>
<email>ur.narmak@gmail.com</email>
</author>
<description>
Empty
</description>
</model>

49
rbs_mill_assist/assets/bunker/model.sdf Normal file
View file

@ -0,0 +1,49 @@
<?xml version="1.0"?>
<sdf version="1.10">
<model name="bunker">
<link name="bunker_link">
<inertial auto="true" />
<visual name="bunker_visual">
<geometry>
<mesh>
<uri>model://bunker/meshes/bunker.stl</uri>
</mesh>
</geometry>
<!-- <material> -->
<!-- <diffuse>1 1 1 1</diffuse> -->
<!-- <ambient>1 1 1 1</ambient> -->
<!-- <specular>0.5 0.5 0.5 1</specular> -->
<!-- <emissive>0 0 0 1</emissive> -->
<!-- <texture> -->
<!-- <diffuse_map>model://bunker/textures/shildik_sh.png</diffuse_map> -->
<!-- </texture> -->
<!-- <pbr> -->
<!-- <metal> -->
<!-- <albedo_map>model://bunker/textures/shildik_sh_d.png</albedo_map> -->
<!-- <normal_map>model://bunker/textures/shildik_sh_n.png</normal_map> -->
<!-- <roughness_map>model://bunker/textures/shildik_sh_r.png</roughness_map> -->
<!-- <metalness_map>model://bunker/textures/shildik_sh_m.png</metalness_map> -->
<!-- <ambient_occlusion_map>model://bunker/textures/shildik_sh_o.png</ambient_occlusion_map> -->
<!-- </metal> -->
<!-- </pbr> -->
<!-- </material> -->
</visual>
<collision name="laser_collision">
<geometry>
<mesh>
<uri>model://bunker/meshes/bunker.stl</uri>
</mesh>
</geometry>
<surface>
<contact>
<ode />
</contact>
<bounce />
<friction>
<ode />
</friction>
</surface>
</collision>
</link>
</model>
</sdf>

BIN
rbs_mill_assist/assets/laser/meshes/laser.stl Normal file
View file

Binary file not shown.

16
rbs_mill_assist/assets/laser/model.config Normal file
View file

@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>laser</name>
<version>0.1</version>
<sdf version='1.10'>model.sdf</sdf>
<author>
<name>Bill Finger</name>
<email>ur.narmak@gmail.com</email>
</author>
<description>
Empty
</description>
</model>

50
rbs_mill_assist/assets/laser/model.sdf Normal file
View file

@ -0,0 +1,50 @@
<?xml version="1.0"?>
<sdf version="1.10">
<model name="laser">
<link name="laser_link">
<pose>0 0 0 1.57 0 0</pose>
<inertial auto="true" />
<visual name="laser_visual">
<geometry>
<mesh>
<uri>model://laser/meshes/laser.stl</uri>
</mesh>
</geometry>
<!-- <material> -->
<!-- <diffuse>1 1 1 1</diffuse> -->
<!-- <ambient>1 1 1 1</ambient> -->
<!-- <specular>0.5 0.5 0.5 1</specular> -->
<!-- <emissive>0 0 0 1</emissive> -->
<!-- <texture> -->
<!-- <diffuse_map>model://laser/textures/shildik_sh.png</diffuse_map> -->
<!-- </texture> -->
<!-- <pbr> -->
<!-- <metal> -->
<!-- <albedo_map>model://laser/textures/shildik_sh_d.png</albedo_map> -->
<!-- <normal_map>model://laser/textures/shildik_sh_n.png</normal_map> -->
<!-- <roughness_map>model://laser/textures/shildik_sh_r.png</roughness_map> -->
<!-- <metalness_map>model://laser/textures/shildik_sh_m.png</metalness_map> -->
<!-- <ambient_occlusion_map>model://laser/textures/shildik_sh_o.png</ambient_occlusion_map> -->
<!-- </metal> -->
<!-- </pbr> -->
<!-- </material> -->
</visual>
<collision name="laser_collision">
<geometry>
<mesh>
<uri>model://laser/meshes/laser.stl</uri>
</mesh>
</geometry>
<surface>
<contact>
<ode />
</contact>
<bounce />
<friction>
<ode />
</friction>
</surface>
</collision>
</link>
</model>
</sdf>

1
rbs_mill_assist/assets/shildik/model.sdf
View file

@ -2,6 +2,7 @@
<sdf version="1.10">
<model name="shildik">
<link name="shildik_link">
<inertial auto="true" />
<visual name="shildik_visual">
<geometry>
<mesh>

33
rbs_mill_assist/bt/CMakeLists.txt Normal file
View file

@ -0,0 +1,33 @@
find_package(behaviortree_ros2 REQUIRED)
find_package(behaviortree_cpp REQUIRED)
find_package(rbs_skill_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
# find_package(std_srvs REQUIRED)
# Behaviortree interfaces
set(dependencies
rclcpp
rbs_skill_interfaces
geometry_msgs
behaviortree_ros2
std_srvs
)
add_library(vacuum_gripper_toggle SHARED plugins/vacuum_gripper_toggle.cpp)
list(APPEND plugin_libs vacuum_gripper_toggle)
add_library(get_grasp_place_pose SHARED plugins/get_grasp_place_pose.cpp)
list(APPEND plugin_libs get_grasp_place_pose)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
endforeach()
install(TARGETS
${plugin_libs}
ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins
LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins
RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins
)

13
rbs_mill_assist/bt/config/bt_executor.yaml Normal file
View file

@ -0,0 +1,13 @@
bt_action_server:
ros__parameters:
action_name: "behavior_server" # Optional (defaults to `bt_action_server`)
tick_frequency: 100 # Optional (defaults to 100 Hz)
groot2_port: 1667 # Optional (defaults to 1667)
ros_plugins_timeout: 1000 # Optional (defaults 1000 ms)
plugins:
- rbs_mill_assist/bt_plugins
- rbs_bt_executor/bt_plugins
behavior_trees:
- rbs_mill_assist/xmls

118
rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp Normal file
View file

@ -0,0 +1,118 @@
#include "behaviortree_ros2/bt_service_node.hpp"
#include "behaviortree_ros2/plugins.hpp"
#include <behaviortree_cpp/basic_types.h>
// #include <geometry_msgs/msg/detail/point__struct.hpp>
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
#include <memory>
// #include <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
#include <rclcpp/logging.hpp>
#include <string>
using GraspingService = rbs_skill_interfaces::srv::GetPickPlacePoses;
using namespace BT;
class Grasping : public RosServiceNode<GraspingService> {
public:
Grasping(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<GraspingService>(name, conf, params) {
RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
}
static PortsList providedPorts() {
return providedBasicPorts(
{InputPort<std::string>("object_name"),
InputPort<std::string>("action_name"),
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pre_pose"),
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("post_pose")});
}
bool setRequest(Request::SharedPtr &request) override {
RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
this->service_name_.c_str());
if (!getInput("action_name", request->action_name)) {
RCLCPP_ERROR(this->node_.lock()->get_logger(),
"Failed to get object_name from input port");
return false;
}
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
RCLCPP_ERROR(this->node_.lock()->get_logger(),
"Response indicates failure.");
return NodeStatus::FAILURE;
}
RCLCPP_INFO(this->node_.lock()->get_logger(),
"Response received successfully.");
auto logPose = [this](const std::string &pose_name,
const geometry_msgs::msg::Pose &pose) {
RCLCPP_INFO(this->node_.lock()->get_logger(),
"%s:\n"
" Position: x = %.4f, y = %.4f, z = %.4f\n"
" Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
pose_name.c_str(), pose.position.x, pose.position.y,
pose.position.z, pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w);
};
if (!response->grasp.empty()) {
RCLCPP_INFO(this->logger(), "Got Pick Response");
std::vector<std::string> poses = {"Pregrasp Pose", "Grasp Pose",
"Postgrasp Pose"};
for (size_t n = 0; n < poses.size() && n < response->grasp.size(); ++n) {
logPose(poses[n], response->grasp.at(n));
}
auto grasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
auto pregrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
auto postgrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
*pregrasp_pose = response->grasp.at(0);
*grasp_pose = response->grasp.at(1);
*postgrasp_pose = response->grasp.at(2);
setOutput("pre_pose", pregrasp_pose);
setOutput("pose", grasp_pose);
setOutput("post_pose", postgrasp_pose);
return NodeStatus::SUCCESS;
}
if (!response->place.empty()) {
RCLCPP_INFO(this->logger(), "Got Place Response");
std::vector<std::string> poses = {"Preplace Pose", "Place Pose",
"Postplace Pose"};
for (size_t n = 0; n < poses.size() && n < response->place.size(); ++n) {
logPose(poses[n], response->place.at(n));
}
auto place_pose = std::make_shared<geometry_msgs::msg::Pose>();
auto preplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
auto postplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
*preplace_pose = response->place.at(0);
*place_pose = response->place.at(1);
*postplace_pose = response->place.at(2);
setOutput("pre_pose", preplace_pose);
setOutput("pose", place_pose);
setOutput("post_pose", postplace_pose);
return NodeStatus::SUCCESS;
}
RCLCPP_FATAL(this->logger(), "Could not response grasp pose");
return NodeStatus::FAILURE;
}
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
};
CreateRosNodePlugin(Grasping, "GetGraspPlacePose");

47
rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp Normal file
View file

@ -0,0 +1,47 @@
#include "behaviortree_ros2/bt_service_node.hpp"
#include "behaviortree_ros2/plugins.hpp"
#include "std_srvs/srv/set_bool.hpp"
#include <behaviortree_cpp/basic_types.h>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>
using ToggleVacuumGripperService = std_srvs::srv::SetBool;
using namespace BT;
class ToggleVacuumGrippper : public RosServiceNode<ToggleVacuumGripperService> {
public:
ToggleVacuumGrippper(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<ToggleVacuumGripperService>(name, conf, params) {
RCLCPP_INFO(this->logger(), "Starting ToggleVacuumGrippper");
}
static PortsList providedPorts() {
return providedBasicPorts({InputPort<bool>("enable")});
}
bool setRequest(Request::SharedPtr &request) override {
RCLCPP_INFO(this->logger(), "Starting send request");
if (!getInput("enable", request->data)) {
RCLCPP_ERROR(this->node_.lock()->get_logger(),
"Failed to get sending data from input port");
return false;
}
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->success) {
RCLCPP_ERROR(this->logger(), "Response indicates failure.");
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
};
CreateRosNodePlugin(ToggleVacuumGrippper, "ToggleVacuumGrippper");

21
rbs_mill_assist/bt/xmls/Grasp.xml Normal file
View file

@ -0,0 +1,21 @@
<?xml version='1.0' encoding='utf-8'?>
<root BTCPP_format="4">
<BehaviorTree ID="Grasp">
<Sequence>
<Script code="action:='/mtp_jtc'"/>
<Script code="get_workspace:='/get_workspace'"/>
<Action ID="GetGraspPlacePose" action_name="pick" pre_pose="{pregrasp}" pose="{grasp}" post_pose="{postgrasp}" service_name="/get_pick_place_poses"/>
<Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="2" action_name="{action}" />
<Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
<Action ID="MoveToPose" pose="{grasp}" robot_name="{robot_name}" duration="2" action_name="{action}" />
<Action ID="MoveToPose" pose="{postgrasp}" robot_name="{robot_name}" duration="2" action_name="{action}" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
</TreeNodesModel>
</root>

9
rbs_mill_assist/bt/xmls/MainTree.xml Normal file
View file

@ -0,0 +1,9 @@
<?xml version='1.0' encoding='utf-8'?>
<root BTCPP_format="4">
<BehaviorTree ID="Main">
<Sequence>
<SubTree ID="Grasp"/>
<SubTree ID="Place"/>
</Sequence>
</BehaviorTree>
</root>

21
rbs_mill_assist/bt/xmls/Place.xml Normal file
View file

@ -0,0 +1,21 @@
<?xml version='1.0' encoding='utf-8'?>
<root BTCPP_format="4">
<BehaviorTree ID="Place">
<Sequence>
<Script code="action:='/mtp_jtc'"/>
<Script code="get_workspace:='/get_workspace'"/>
<Action ID="GetGraspPlacePose" action_name="place" pre_pose="{preplace}" pose="{place}" post_pose="{postplace}" service_name="/get_pick_place_poses"/>
<Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="2" action_name="{action}" />
<Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="{action}" />
<Action ID="ToggleVacuumGrippper" enable="false" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
<Action ID="MoveToPose" pose="{postplace}" robot_name="{robot_name}" duration="2" action_name="{action}" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
</TreeNodesModel>
</root>

60
rbs_mill_assist/config/grasping.yaml Normal file
View file

@ -0,0 +1,60 @@
pregrasp_pose:
position:
x: 0.10395
y: -0.28
z: 0.1
orientation:
x: 0.0
y: 1.0
z: 0.0
w: 0.0
grasp_pose:
position:
x: 0.10395
y: -0.28
z: 0.004
orientation:
x: 0.0
y: 1.0
z: 0.0
w: 0.04
postgrasp_pose:
position:
x: 0.10395
y: -0.28
z: 0.1
orientation:
x: 0.0
y: 1.0
z: 0.0
w: 0.0
preplace_pose:
position:
x: 0.360
y: -0.06
z: 0.1
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0
place_pose:
position:
x: 0.360
y: -0.06
z: 0.05
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0
postplace_pose:
position:
x: 0.360
y: -0.06
z: 0.1
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0

23
rbs_mill_assist/launch/bt_executor.launch.py Normal file
View file

@ -0,0 +1,23 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
executor_params = PathJoinSubstitution([FindPackageShare("rbs_mill_assist"), "config", "bt_executor.yaml"])
nodes_to_start = [
Node(
package='rbs_bt_executor',
executable='rbs_bt_executor',
# prefix=['gdbserver localhost:1234'],
parameters=[
executor_params,
{'use_sim_time': True}
],
)
]
return LaunchDescription(nodes_to_start)

12
rbs_mill_assist/launch/simulation.launch.py
View file

@ -165,12 +165,18 @@ def launch_setup(context, *args, **kwargs):
config_file=os.path.join(description_package_abs_path, "config", "gz_bridge.yaml")
)
grasping_service = Node(
package="rbs_mill_assist",
executable="grasping_service.py"
)
nodes_to_start = [
rbs_robot_setup,
rviz_node,
gazebo,
gazebo_spawn_robot,
gz_bridge
gz_bridge,
grasping_service
]
return nodes_to_start
@ -272,7 +278,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"ee_link_name",
default_value="Link6",
default_value="grasp_point",
description="End effector name of robot arm",
)
)
@ -302,7 +308,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"interactive",
default_value="true",
default_value="false",
description="Wheter to run the motion_control_handle controller",
),
)

1
rbs_mill_assist/package.xml
View file

@ -19,6 +19,7 @@
<depend>cartesian_force_controller</depend>
<depend>cartesian_motion_controller</depend>
<depend>cartesian_twist_controller</depend>
<depend>rbs_bt_executor</depend>
<test_depend>ament_lint_auto</test_depend>

4
rbs_mill_assist/scripts/CMakeLists.txt Normal file
View file

@ -0,0 +1,4 @@
install(PROGRAMS
grasping_service.py
DESTINATION lib/${PROJECT_NAME}
)

101
rbs_mill_assist/scripts/grasping_service.py Executable file
View file

@ -0,0 +1,101 @@
#!/usr/bin/env python3
import os
from typing import Dict
import rclpy
import yaml
from ament_index_python.packages import get_package_share_directory
from geometry_msgs.msg import Pose
from rbs_skill_interfaces.srv import GetPickPlacePoses
from rclpy.node import Node
from rclpy.service import Service
class GetGraspPickPoses(Node):
def __init__(self) -> None:
super().__init__("get_grasp_pick_poses")
self.srv: Service = self.create_service(
GetPickPlacePoses, "get_pick_place_poses", self.get_grasp_pick_poses
)
yaml_file_path: str = os.path.join(
get_package_share_directory("rbs_mill_assist"), "config", "grasping.yaml"
)
with open(yaml_file_path, "r", encoding="utf-8") as file:
self.grasping: Dict = yaml.safe_load(file)
def create_pose(self, pose_data: Dict) -> Pose:
"""
Helper function to create a Pose from the given pose data.
"""
pose = Pose()
if pose_data:
pose.position.x = pose_data.get("position", {}).get("x", 0.0)
pose.position.y = pose_data.get("position", {}).get("y", 0.0)
pose.position.z = pose_data.get("position", {}).get("z", 0.0)
pose.orientation.x = pose_data.get("orientation", {}).get("x", 0.0)
pose.orientation.y = pose_data.get("orientation", {}).get("y", 0.0)
pose.orientation.z = pose_data.get("orientation", {}).get("z", 0.0)
pose.orientation.w = pose_data.get("orientation", {}).get("w", 1.0)
return pose
def get_grasp_pick_poses(
self, request: GetPickPlacePoses.Request, response: GetPickPlacePoses.Response
) -> GetPickPlacePoses.Response:
if request.action_name == "pick":
pregrasp_pose = self.grasping.get("pregrasp_pose", None)
grasp_pose = self.grasping.get("grasp_pose", None)
postgrasp_pose = self.grasping.get("postgrasp_pose", None)
if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
response.ok = False
self.get_logger().error(
"Missing one or more of the pregrasp, grasp, or postgrasp poses."
)
return response
# Create Pose messages
response.grasp = [
self.create_pose(pregrasp_pose),
self.create_pose(grasp_pose),
self.create_pose(postgrasp_pose),
]
elif request.action_name == "place":
preplace_pose = self.grasping.get("preplace_pose", None)
place_pose = self.grasping.get("place_pose", None)
postplace_pose = self.grasping.get("postplace_pose", None)
if None in [preplace_pose, place_pose, postplace_pose]:
response.ok = False
self.get_logger().error(
"Missing one or more of the preplace, place, or postplace poses."
)
return response
# Create Pose messages for placing
response.place = [
self.create_pose(preplace_pose),
self.create_pose(place_pose),
self.create_pose(postplace_pose),
]
response.ok = True
self.get_logger().info(f"Handled action: {request.action_name}")
return response
def main():
rclpy.init()
executor = rclpy.executors.SingleThreadedExecutor()
# executor = rclpy.executors.MultiThreadedExecutor() # can't be used
i_node = GetGraspPickPoses()
executor.add_node(i_node)
try:
executor.spin()
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
i_node.destroy_node()
if __name__ == "__main__":
main()

2
rbs_mill_assist/urdf/rbs_arm.xacro
View file

@ -8,7 +8,7 @@
<xacro:arg name="x" default="-0.10" />
<xacro:arg name="y" default="0.0" />
<xacro:arg name="z" default="0.03" />
<xacro:arg name="z" default="0.0" />
<xacro:arg name="roll" default="0.0" />
<xacro:arg name="pitch" default="0.0" />

55
rbs_mill_assist/urdf/rbs_arm_macro.xacro
View file

@ -160,7 +160,7 @@
<child
link="Link2" />
<axis
xyz="-1 0 0" />
xyz="1 0 0" />
<limit
lower="-2.2689"
upper="1.7453"
@ -276,7 +276,7 @@
<child
link="Link4" />
<axis
xyz="0 0 -1" />
xyz="0 0 1" />
<limit
lower="-6.2832"
upper="6.2832"
@ -505,17 +505,45 @@
<axis
xyz="0 0 0" />
</joint>
<link name="grasp_point"/>
<joint
name="grasp_point_joint"
type="fixed">
<origin
xyz="0.0 -0.095 0.05"
rpy="0 0 0" />
<parent
link="Link6" />
<child
link="grasp_point" />
<axis
xyz="0 0 0" />
</joint>
<xacro:macro name="gazebo_reference" params="joint_name">
<gazebo reference='${joint_name}'>
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>
</xacro:macro>
<xacro:gazebo_reference joint_name="Right_point"/>
<xacro:gazebo_reference joint_name="Left_point"/>
<xacro:joint_hardware joint_name="ax1" hardware="${hardware}" initial_joint_position="0.0" />
<xacro:joint_hardware joint_name="ax2" hardware="${hardware}" initial_joint_position="0.0" />
<xacro:joint_hardware joint_name="ax3" hardware="${hardware}" initial_joint_position="0.0" />
<xacro:joint_hardware joint_name="ax2" hardware="${hardware}" initial_joint_position="0.85" />
<xacro:joint_hardware joint_name="ax3" hardware="${hardware}" initial_joint_position="1.0" />
<xacro:joint_hardware joint_name="ax4" hardware="${hardware}" initial_joint_position="0.0" />
<xacro:joint_hardware joint_name="ax5" hardware="${hardware}" initial_joint_position="0.0" />
<xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="0.0" />
<xacro:joint_hardware joint_name="ax5" hardware="${hardware}" initial_joint_position="1.0" />
<xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="3.14159" />
<xacro:if value="${hardware=='gazebo'}">
<!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
<xacro:rgbd parent="Link1" tf_prefix="${tf_prefix}">
<origin rpy="0.0 0.0 -1.57" xyz="0.0 -0.035 0.03"></origin>
</xacro:rgbd>
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters>$(arg simulation_controllers)</parameters>
@ -524,14 +552,21 @@
</ros>
</plugin>
<plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
<link_name>Link6</link_name>
<link_name>Link8</link_name>
</plugin>
</gazebo>
</xacro:if>
<gazebo reference="Link6">
<sensor name='sensor_contact' type='contact'>
<gazebo reference="Link8">
<sensor name='left_sensor_contact' type='contact'>
<contact>
<collision>Link6_collision</collision>
<collision>Link8_collision</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="Link7">
<sensor name='right_sensor_contact' type='contact'>
<contact>
<collision>Link7_collision</collision>
</contact>
</sensor>
</gazebo>

62
rbs_mill_assist/urdf/rbs_arm_modular.xacro
View file

@ -1,62 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rbs_arm">
<xacro:arg name="gripper_name" default="" />
<xacro:arg name="hardware" default="gazebo" />
<xacro:arg name="simulation_controllers" default="" />
<xacro:arg name="tf_prefix" default="" />
<xacro:arg name="namespace" default="arm0"/>
<xacro:arg name="x" default="-0.10" />
<xacro:arg name="y" default="0.0" />
<xacro:arg name="z" default="0.03" />
<xacro:arg name="roll" default="0.0" />
<xacro:arg name="pitch" default="0.0" />
<xacro:arg name="yaw" default="0.0" />
<xacro:include filename="$(find rbs_mill_assist)/urdf/rbs_arm_modular_macro.xacro" />
<link name="world" />
<xacro:property name="machine_length" value="0.4"/>
<xacro:property name="machine_width" value="0.5"/>
<xacro:property name="machine_height" value="0.115"/>
<link name="machine_link">
<visual>
<geometry>
<box size="${machine_length} ${machine_width} ${machine_height}"/>
</geometry>
<material name="gray">
<color rgba="0 1 0 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="${machine_length} ${machine_width} ${machine_height}"/>
</geometry>
</collision>
<inertial>
<mass value="40.0"/>
<inertia
ixx="${(1/12)*30*(machine_width**2 + machine_height**2)}"
iyy="${(1/12)*30*(machine_length**2 + machine_height**2)}"
izz="${(1/12)*30*(machine_length**2 + machine_width**2)}"
ixy="0"
ixz="0"
iyz="0"/>
</inertial>
</link>
<joint name="machine_joint" type="fixed">
<parent link="world"/>
<child link="machine_link"/>
<origin xyz="0.25 0 ${0.115/2}" rpy="0 0 0"/>
</joint>
<!-- Arm-->
<xacro:rbs_arm namespace="$(arg namespace)" parent="world" tf_prefix="$(arg tf_prefix)" hardware="$(arg hardware)"
gripper_name="$(arg gripper_name)" controllers="$(arg simulation_controllers)">
<origin xyz="$(arg x) $(arg y) $(arg z)" rpy="$(arg roll) $(arg pitch) $(arg yaw)" />
</xacro:rbs_arm>
</robot>

63
rbs_mill_assist/urdf/rbs_arm_modular_macro.xacro
View file

@ -1,63 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="rbs_arm" params="namespace parent *origin tf_prefix hardware gripper_name controllers">
<xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_base_link.xacro"></xacro:include>
<xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_ee_link.xacro"></xacro:include>
<xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_fork_link.xacro"></xacro:include>
<xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_main_link.xacro"></xacro:include>
<xacro:include filename="$(find rbs_arm)/urdf/sensors.xacro"></xacro:include>
<!-- BEGIN robot description -->
<!-- link 0-->
<xacro:base_link hardware="${hardware}" parent="${parent}" tf_prefix="${tf_prefix}">
<xacro:insert_block name="origin"></xacro:insert_block>
</xacro:base_link>
<!-- link 1-->
<xacro:fork_link d="0.2" hardware="${hardware}" name="fork0_link" p="10000" parent="base_link" tf_prefix="${tf_prefix}" initial_joint_position="1.57"></xacro:fork_link>
<!-- link 2-->
<xacro:main_link d="0.2" hardware="${hardware}" name="main0_link" p="10000" parent="fork0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.5"></xacro:main_link>
<!-- link 3-->
<xacro:fork_link d="0.2" hardware="${hardware}" name="fork1_link" p="10000" parent="main0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.0"></xacro:fork_link>
<!-- link 4-->
<xacro:main_link d="0.2" hardware="${hardware}" name="main1_link" p="1000" parent="fork1_link" tf_prefix="${tf_prefix}" initial_joint_position="1.5"></xacro:main_link>
<!-- ee link also contain tool0-->
<!-- link 5-->
<xacro:ee_link d="0.2" hardware="${hardware}" p="20" parent="main1_link" tf_prefix="${tf_prefix}"></xacro:ee_link>
<!-- END robot description -->
<xacro:rgbd parent="${tf_prefix}ee_link" tf_prefix="${tf_prefix}">
<origin rpy="0.0 -1.57 0.0" xyz="0.0 0.0 0.0754"></origin>
</xacro:rgbd>
<!-- Insert gripper in robot description if exist -->
<!-- <xacro:if value="${gripper_name=='rbs_gripper'}"> -->
<!-- <xacro:include filename="$(find rbs_gripper)/urdf/rbs_gripper_macro.xacro"></xacro:include> -->
<!-- <xacro:rbs_gripper gripper_name="rbs_gripper" hardware="${hardware}"
parent="${tf_prefix}tool0" tf_prefix="${tf_prefix}"> -->
<!-- <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"></origin> -->
<!-- </xacro:rbs_gripper> -->
<!-- </xacro:if> -->
<!-- If hardware == gazebo so insert additional parameters for it -->
<xacro:if value="${hardware=='gazebo'}">
<!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters>$(arg simulation_controllers)</parameters>
<ros>
<namespace>${namespace}</namespace>
</ros>
</plugin>
<plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
<link_name>ee_link</link_name>
</plugin>
</gazebo>
</xacro:if>
<gazebo reference="ee_link">
<sensor name='sensor_contact' type='contact'>
<contact>
<collision>ee_link_collision</collision>
</contact>
</sensor>
</gazebo>
</xacro:macro>
</robot>

37
rbs_mill_assist/world/default.sdf
View file

@ -10,7 +10,7 @@
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<dart>
<collision_detector>bullet</collision_detector>
<!-- <collision_detector>bullet</collision_detector> -->
<!-- <solver> -->
<!-- <solver_type>pgs</solver_type> -->
<!-- </solver> -->
@ -55,9 +55,42 @@
</light>
<include>
<pose>-0.35 0.0 0 0 0 0</pose>
<name>shildik_0</name>
<pose>0.0 -0.28 0.0 0 0 3.14159</pose>
<uri>model://shildik</uri>
</include>
<include>
<name>shildik_1</name>
<pose>0.0 -0.28 0.01 0 0 3.14159</pose>
<uri>model://shildik</uri>
</include>
<include>
<name>shildik_2</name>
<pose>0.0 -0.28 0.02 0 0 3.14159</pose>
<uri>model://shildik</uri>
</include>
<include>
<name>shildik_3</name>
<pose>0.0 -0.28 0.03 0 0 3.14159</pose>
<uri>model://shildik</uri>
</include>
<include>
<name>shildik_4</name>
<pose>0.0 -0.28 0.04 0 0 3.14159</pose>
<uri>model://shildik</uri>
</include>
<include>
<static>true</static>
<pose>0.30 0.0 0 0 0 -1.57</pose>
<uri>model://laser</uri>
</include>
<!-- <include> -->
<!-- <static>true</static> -->
<!-- <pose>0.0 -0.3 0.0752 1.57 0 3.14159</pose> -->
<!-- <uri>model://bunker</uri> -->
<!-- </include> -->
<!-- <model name="ground_plane"> -->
<!-- <static>true</static> -->