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

Update BT for full process with graver

Browse source 
- Adding new fixed joint links (`right_contact_panel` and `left_contact_panel`) to Link8 and Link7.
 - Defining visual representations with semi-transparent boxes for these panels.
 - Setting up collision models to enable accurate contact detection.
 - Updating Gazebo references to maintain fixed joint properties in simulations.
 - Moving sensors from the main links to the new contact panels for better interaction sensing.
 - Correcting and updating collision references in sensors.
 - Making minor XML formatting adjustments for consistency.
This commit is contained in:
Ilya Uraev 2025-03-03 16:36:35 +03:00
parent f5b9924b79
commit 2156e9a0c9
17 changed files with 543 additions and 121 deletions
Download patch file Download diff file Expand all files Collapse all files

36
rbs_mill_assist/assets/laser/model.sdf
View file

@ -10,24 +10,24 @@
<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> -->
<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>

71
rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae
View file

@ -3,13 +3,66 @@
<asset>
<contributor>
<author>Blender User</author>
<authoring_tool>Blender 4.1.1 commit date:2024-04-15, commit time:15:11, hash:e1743a0317bc</authoring_tool>
<authoring_tool>Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a</authoring_tool>
</contributor>
<created>2025-02-12T10:55:51</created>
<modified>2025-02-12T10:55:51</modified>
<created>2025-03-03T12:34:37</created>
<modified>2025-03-03T12:34:37</modified>
<unit name="meter" meter="1"/>
<up_axis>Z_UP</up_axis>
</asset>
<library_cameras>
<camera id="Camera-camera" name="Camera">
<optics>
<technique_common>
<perspective>
<xfov sid="xfov">39.59775</xfov>
<aspect_ratio>1.777778</aspect_ratio>
<znear sid="znear">0.1</znear>
<zfar sid="zfar">100</zfar>
</perspective>
</technique_common>
</optics>
<extra>
<technique profile="blender">
<shiftx sid="shiftx" type="float">0</shiftx>
<shifty sid="shifty" type="float">0</shifty>
<dof_distance sid="dof_distance" type="float">10</dof_distance>
</technique>
</extra>
</camera>
</library_cameras>
<library_lights>
<light id="Light-light" name="Light">
<technique_common>
<point>
<color sid="color">1000 1000 1000</color>
<constant_attenuation>1</constant_attenuation>
<linear_attenuation>0</linear_attenuation>
<quadratic_attenuation>0</quadratic_attenuation>
</point>
</technique_common>
<extra>
<technique profile="blender">
<type sid="type" type="int">0</type>
<flag sid="flag" type="int">0</flag>
<mode sid="mode" type="int">2097153</mode>
<red sid="red" type="float">1</red>
<green sid="green" type="float">1</green>
<blue sid="blue" type="float">1</blue>
<energy sid="blender_energy" type="float">1000</energy>
<spotsize sid="spotsize" type="float">75</spotsize>
<spotblend sid="spotblend" type="float">0.15</spotblend>
<clipsta sid="clipsta" type="float">0.04999995</clipsta>
<clipend sid="clipend" type="float">40</clipend>
<radius sid="radius" type="float">0.1</radius>
<area_shape sid="area_shape" type="int">1</area_shape>
<area_size sid="area_size" type="float">0.1</area_size>
<area_sizey sid="area_sizey" type="float">0.1</area_sizey>
<area_sizez sid="area_sizez" type="float">1</area_sizez>
</technique>
</extra>
</light>
</library_lights>
<library_effects>
<effect id="nasosnaya_ustanovka_svg-effect">
<profile_COMMON>
@ -39,7 +92,7 @@
<geometry id="shildik-mesh" name="shildik">
<mesh>
<source id="shildik-mesh-positions">
<float_array id="shildik-mesh-positions-array" count="54">-0.02999997 -0.01999998 0 -0.02999997 -0.01999998 5e-4 -0.02999997 0.01999998 0 -0.02999997 0.01999998 5e-4 0.02999997 -0.01999998 0 0.02999997 -0.01999998 5e-4 0.02999997 0.01999998 0 0.02999997 0.01999998 5e-4 -0.02999997 0 0 -0.02999997 0 5e-4 0 0.01999998 0 0 0.01999998 5e-4 0.02999997 0 0 0.02999997 0 5e-4 0 -0.01999998 0 0 -0.01999998 5e-4 0 0 5e-4 0 0 0</float_array>
<float_array id="shildik-mesh-positions-array" count="54">-0.02999997 -0.01999998 -2.5e-4 -0.02999997 -0.01999998 2.5e-4 -0.02999997 0.01999998 -2.5e-4 -0.02999997 0.01999998 2.5e-4 0.02999997 -0.01999998 -2.5e-4 0.02999997 -0.01999998 2.5e-4 0.02999997 0.01999998 -2.5e-4 0.02999997 0.01999998 2.5e-4 -0.02999997 0 -2.5e-4 -0.02999997 0 2.5e-4 0 0.01999998 -2.5e-4 0 0.01999998 2.5e-4 0.02999997 0 -2.5e-4 0.02999997 0 2.5e-4 0 -0.01999998 -2.5e-4 0 -0.01999998 2.5e-4 0 0 2.5e-4 0 0 -2.5e-4</float_array>
<technique_common>
<accessor source="#shildik-mesh-positions-array" count="18" stride="3">
<param name="X" type="float"/>
@ -87,12 +140,20 @@
<bind_material>
<technique_common>
<instance_material symbol="nasosnaya_ustanovka_svg-material" target="#nasosnaya_ustanovka_svg-material">
<bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
<bind_vertex_input semantic="shildik-mesh-map-0" input_semantic="TEXCOORD" input_set="0"/>
</instance_material>
</technique_common>
</bind_material>
</instance_geometry>
</node>
<node id="Camera" name="Camera" type="NODE">
<matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
<instance_camera url="#Camera-camera"/>
</node>
<node id="Light" name="Light" type="NODE">
<matrix sid="transform">-0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
<instance_light url="#Light-light"/>
</node>
</visual_scene>
</library_visual_scenes>
<scene>

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

@ -31,7 +31,7 @@
<collision name="shildik_collision">
<geometry>
<box>
<size>0.06 0.04 0.001</size>
<size>0.06 0.04 0.0005</size>
</box>
</geometry>
<surface>

5
rbs_mill_assist/bt/CMakeLists.txt
View file

@ -2,6 +2,7 @@
find_package(behaviortree_ros2 REQUIRED)
find_package(behaviortree_cpp REQUIRED)
find_package(rbs_skill_interfaces REQUIRED)
find_package(rbs_utils_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
# find_package(std_srvs REQUIRED)
@ -11,6 +12,7 @@ set(dependencies
rbs_skill_interfaces
geometry_msgs
behaviortree_ros2
rbs_utils_interfaces
std_srvs
)
@ -20,6 +22,9 @@ 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)
add_library(get_named_pose SHARED plugins/get_named_pose.cpp)
list(APPEND plugin_libs get_named_pose)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

63
rbs_mill_assist/bt/plugins/get_named_pose.cpp Normal file
View file

@ -0,0 +1,63 @@
#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 "rbs_utils_interfaces/srv/get_named_pose.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 <rclcpp/logging.hpp>
#include <string>
using GetNamedPoseService = rbs_utils_interfaces::srv::GetNamedPose;
using namespace BT;
class GetNamedPose : public RosServiceNode<GetNamedPoseService> {
public:
GetNamedPose(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<GetNamedPoseService>(name, conf, params) {
RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
}
static PortsList providedPorts() {
return providedBasicPorts(
{InputPort<std::string>("pose_name"),
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output_pose")});
}
bool setRequest(Request::SharedPtr &request) override {
RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
this->service_name_.c_str());
if (!getInput("pose_name", request->pose_name)) {
RCLCPP_ERROR(this->node_.lock()->get_logger(),
"Failed to get pose_name from input port");
return false;
}
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
RCLCPP_ERROR(this->logger(), "Response indicates failure.");
return NodeStatus::FAILURE;
}
RCLCPP_INFO(this->logger(),
"Response received successfully with pose name [%s]",
response->named_pose.name.c_str());
auto pose = std::make_shared<geometry_msgs::msg::Pose>();
*pose = response->named_pose.pose;
setOutput("output_pose", pose);
return NodeStatus::SUCCESS;
}
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
};
CreateRosNodePlugin(GetNamedPose, "GetNamedPose");

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

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

14
rbs_mill_assist/bt/xmls/Grasp.xml
View file

@ -2,16 +2,12 @@
<root BTCPP_format="4">
<BehaviorTree ID="Grasp">
<Sequence>
<Action ID="GetGraspPlacePose" action_name="{action_name}" pre_pose="{pregrasp}" pose="{grasp}" post_pose="{postgrasp}" service_name="/get_pick_place_poses" />
<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}" />
<Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc" />
<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="/mtp_jtc_cart" />
<Action ID="MoveToPose" pose="{postgrasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
</Sequence>
</BehaviorTree>

15
rbs_mill_assist/bt/xmls/MainTree.xml
View file

@ -2,8 +2,19 @@
<root BTCPP_format="4">
<BehaviorTree ID="Main">
<Sequence>
<SubTree ID="Grasp"/>
<SubTree ID="Place"/>
<SubTree ID="ToGraver" />
<Action ID="GetNamedPose" pose_name="waiting" output_pose="{named_pose}"
service_name="/get_named_pose" />
<Action ID="MoveToPose" pose="{named_pose}" robot_name="{robot_name}" duration="2"
action_name="/mtp_jtc" />
<Delay delay_msec="2000">
<SubTree ID="FromGraver"/>
</Delay>
<Action ID="MoveToJointState" joint_state="0.0;0.85;1.0;0.0;1.0;3.14159" duration="3" robot_name="arm0" action_name="/mtjs_jtc"/>
</Sequence>
</BehaviorTree>
</root>

14
rbs_mill_assist/bt/xmls/Place.xml
View file

@ -2,16 +2,12 @@
<root BTCPP_format="4">
<BehaviorTree ID="Place">
<Sequence>
<Action ID="GetGraspPlacePose" action_name="{action_name}" pre_pose="{preplace}" pose="{place}" post_pose="{postplace}" service_name="/get_pick_place_poses" />
<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}" />
<Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc" />
<Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
<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="/mtp_jtc_cart" />
</Sequence>
</BehaviorTree>

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

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

183
rbs_mill_assist/config/grasping.yaml
View file

@ -1,60 +1,123 @@
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
to_graver:
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.001
orientation:
x: 0.0
y: 1.0
z: 0.0
w: 0.0
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.330
y: -0.06
z: 0.1
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0
place_pose:
position:
x: 0.330
y: -0.06
z: 0.01
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0
postplace_pose:
position:
x: 0.330
y: -0.06
z: 0.1
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0
from_graver:
pregrasp_pose:
position:
x: 0.330
y: -0.06
z: 0.1
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0
grasp_pose:
position:
x: 0.330
y: -0.06
z: 0.0
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0
postgrasp_pose:
position:
x: 0.330
y: -0.06
z: 0.1
orientation:
x: -0.707
y: 0.707
z: 0.0
w: 0.0
preplace_pose:
position:
x: 0.10395
y: 0.28
z: 0.1
orientation:
x: -1.000
y: 0.0
z: 0.0
w: 0.0
place_pose:
position:
x: 0.10395
y: 0.28
z: 0.000
orientation:
x: -1.000
y: 0.0
z: 0.0
w: 0.0
postplace_pose:
position:
x: 0.10395
y: 0.28
z: 0.1
orientation:
x: -1.000
y: 0.0
z: 0.0
w: 0.0

10
rbs_mill_assist/config/key_poses.yaml Normal file
View file

@ -0,0 +1,10 @@
waiting:
position:
x: 0.2116573005914688
y: -0.10102221369743347
z: 0.1460307389497757
orientation:
x: 0.0
y: 1.0
z: 0.0
w: 0.0

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

@ -143,7 +143,7 @@ def launch_setup(context, *args, **kwargs):
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_gz_sim'),
'launch', 'gz_sim.launch.py')]),
launch_arguments=[('gz_args', [' -r -v 4 ', gazebo_world])],
launch_arguments=[('gz_args', [' -r ', gazebo_world])],
)
gazebo_spawn_robot = Node(
@ -170,13 +170,19 @@ def launch_setup(context, *args, **kwargs):
executable="grasping_service.py"
)
get_named_pose_service = Node(
package="rbs_mill_assist",
executable="get_key_pose_frame.py"
)
nodes_to_start = [
rbs_robot_setup,
rviz_node,
gazebo,
gazebo_spawn_robot,
gz_bridge,
grasping_service
grasping_service,
get_named_pose_service
]
return nodes_to_start

1
rbs_mill_assist/scripts/CMakeLists.txt
View file

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

72
rbs_mill_assist/scripts/get_key_pose_frame.py Executable file
View file

@ -0,0 +1,72 @@
#!/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_utils_interfaces.srv import GetNamedPose
from rclpy.node import Node
from rclpy.service import Service
class GetNamedPoseService(Node):
def __init__(self) -> None:
super().__init__("get_named_pose_service")
self.srv: Service = self.create_service(
GetNamedPose, "get_named_pose", self.get_named_pose
)
yaml_file_path: str = os.path.join(
get_package_share_directory("rbs_mill_assist"), "config", "key_poses.yaml"
)
with open(yaml_file_path, "r", encoding="utf-8") as file:
self.key_poses: 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_named_pose(
self, request: GetNamedPose.Request, response: GetNamedPose.Response
) -> GetNamedPose.Response:
key_pose = self.key_poses.get(request.pose_name, None)
if key_pose is None:
response.ok = False
return response
response.named_pose.name = request.pose_name
response.named_pose.pose = self.create_pose(key_pose)
response.ok = True
return response
def main():
rclpy.init()
executor = rclpy.executors.SingleThreadedExecutor()
# executor = rclpy.executors.MultiThreadedExecutor() # can't be used
i_node = GetNamedPoseService()
executor.add_node(i_node)
try:
executor.spin()
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
i_node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

55
rbs_mill_assist/scripts/grasping_service.py
View file

@ -42,10 +42,11 @@ class GetGraspPickPoses(Node):
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 request.action_name == "to_graver.pick":
to_graver = self.grasping.get("to_graver", None)
pregrasp_pose = to_graver.get("pregrasp_pose", None)
grasp_pose = to_graver.get("grasp_pose", None)
postgrasp_pose = to_graver.get("postgrasp_pose", None)
if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
response.ok = False
@ -60,11 +61,49 @@ class GetGraspPickPoses(Node):
self.create_pose(grasp_pose),
self.create_pose(postgrasp_pose),
]
elif request.action_name == "to_graver.place":
to_graver = self.grasping.get("to_graver", None)
preplace_pose = to_graver.get("preplace_pose", None)
place_pose = to_graver.get("place_pose", None)
postplace_pose = to_graver.get("postplace_pose", None)
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),
]
elif request.action_name == "from_graver.pick":
from_graver = self.grasping.get("from_graver", None)
pregrasp_pose = from_graver.get("pregrasp_pose", None)
grasp_pose = from_graver.get("grasp_pose", None)
postgrasp_pose = from_graver.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 == "from_graver.place":
from_graver = self.grasping.get("from_graver", None)
preplace_pose = from_graver.get("preplace_pose", None)
place_pose = from_graver.get("place_pose", None)
postplace_pose = from_graver.get("postplace_pose", None)
if None in [preplace_pose, place_pose, postplace_pose]:
response.ok = False

95
rbs_mill_assist/urdf/rbs_arm_macro.xacro
View file

@ -505,7 +505,7 @@
<axis
xyz="0 0 0" />
</joint>
<link name="grasp_point"/>
<link name="grasp_point" />
<joint
name="grasp_point_joint"
type="fixed">
@ -520,14 +520,95 @@
xyz="0 0 0" />
</joint>
<xacro:property name="contact_panel_mass" value="0.0198935071603267" />
<xacro:property name="box_x" value="0.01" />
<xacro:property name="box_y" value="0.01" />
<xacro:property name="box_z" value="0.001" />
<xacro:property name="contact_panel_ixx"
value="${(1/12.0) * contact_panel_mass * (box_y*box_y + box_z*box_z)}" />
<xacro:property name="contact_panel_iyy"
value="${(1/12.0) * contact_panel_mass * (box_x*box_x + box_z*box_z)}" />
<xacro:property name="contact_panel_izz"
value="${(1/12.0) * contact_panel_mass * (box_x*box_x + box_y*box_y)}" />
<link name="right_contact_panel">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="${contact_panel_mass}" />
<inertia
ixx="${contact_panel_ixx}"
ixy="0"
ixz="0"
iyy="${contact_panel_iyy}"
iyz="0"
izz="${contact_panel_izz}" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${box_x} ${box_y} ${box_z}" />
</geometry>
<material name="">
<color rgba="1 1 1 0.4" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${box_x} ${box_y} ${box_z}" />
</geometry>
</collision>
</link>
<link name="left_contact_panel">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="${contact_panel_mass}" />
<inertia
ixx="${contact_panel_ixx}"
ixy="0"
ixz="0"
iyy="${contact_panel_iyy}"
iyz="0"
izz="${contact_panel_izz}" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${box_x} ${box_y} ${box_z}" />
</geometry>
<material name="">
<color rgba="1 1 1 0.4" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${box_x} ${box_y} ${box_z}" />
</geometry>
</collision>
</link>
<joint name="right_contact_panel_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0" />
<parent link="Link8" />
<child link="right_contact_panel" />
</joint>
<joint name="left_contact_panel_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0" />
<parent link="Link7" />
<child link="left_contact_panel" />
</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:gazebo_reference joint_name="Right_point" />
<xacro:gazebo_reference joint_name="Left_point" />
<xacro:gazebo_reference joint_name="right_contact_panel_joint" />
<xacro:gazebo_reference joint_name="left_contact_panel_joint" />
<xacro:joint_hardware joint_name="ax1" hardware="${hardware}" initial_joint_position="0.0" />
@ -556,17 +637,17 @@
</plugin>
</gazebo>
</xacro:if>
<gazebo reference="Link8">
<gazebo reference="left_contact_panel">
<sensor name='left_sensor_contact' type='contact'>
<contact>
<collision>Link8_collision</collision>
<collision>left_contact_panel_collision</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="Link7">
<gazebo reference="right_contact_panel">
<sensor name='right_sensor_contact' type='contact'>
<contact>
<collision>Link7_collision</collision>
<collision>right_contact_panel_collision</collision>
</contact>
</sensor>
</gazebo>