update robot gripper and current scene
- added bunker with 4 slots and refactored bunker
This commit is contained in:
parent
4160b2f935
commit
9780e44c1e
14 changed files with 239 additions and 146 deletions
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -6,7 +6,7 @@
|
|||
<visual name="bunker_visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://bunker/meshes/Bunker_for_labels_60х40.STL</uri>
|
||||
<uri>model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -14,12 +14,16 @@
|
|||
<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://shildik/textures/shildik_sh.png</diffuse_map> -->
|
||||
<!-- </texture> -->
|
||||
</material>
|
||||
</visual>
|
||||
<collision name="bunker_collision">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://bunker/meshes/Bunker_for_labels_60х40.STL</uri>
|
||||
<uri>model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
|
|
|
|||
Binary file not shown.
16
rbs_mill_assist/assets/bunker_4_slots/model.config
Normal file
16
rbs_mill_assist/assets/bunker_4_slots/model.config
Normal file
|
|
@ -0,0 +1,16 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<model>
|
||||
<name>bunker_4_slots</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>
|
||||
53
rbs_mill_assist/assets/bunker_4_slots/model.sdf
Normal file
53
rbs_mill_assist/assets/bunker_4_slots/model.sdf
Normal file
|
|
@ -0,0 +1,53 @@
|
|||
<?xml version="1.0"?>
|
||||
<sdf version="1.10">
|
||||
<model name="bunker_4_slots">
|
||||
<link name="bunker_4_slots_link">
|
||||
<inertial auto="true" />
|
||||
<visual name="bunker_4_slots_visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.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://shildik/textures/shildik_sh.png</diffuse_map> -->
|
||||
<!-- </texture> -->
|
||||
</material>
|
||||
</visual>
|
||||
<collision name="bunker_4_slots_collision">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>1e6</kp>
|
||||
<kd>1e3</kd>
|
||||
<max_vel>0.1</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
<bounce>
|
||||
<restitution>0.3</restitution>
|
||||
</bounce>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>0.61</mu>
|
||||
<mu2>0.47</mu2>
|
||||
<slip1>0.0</slip1>
|
||||
<slip2>0.0</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,3 +1,5 @@
|
|||
#include "geometry_msgs/msg/pose.hpp"
|
||||
#include "moveit_msgs/msg/collision_object.hpp"
|
||||
#include "moveit_msgs/msg/planning_scene.hpp"
|
||||
#include "shape_msgs/msg/mesh.hpp"
|
||||
#include <chrono>
|
||||
|
|
@ -11,6 +13,7 @@
|
|||
#include <shape_msgs/msg/solid_primitive.hpp>
|
||||
#include <shape_msgs/msg/mesh.hpp>
|
||||
#include <geometric_shapes/shape_operations.h>
|
||||
#include <stdexcept>
|
||||
|
||||
class PlanningScenePublisher : public rclcpp::Node {
|
||||
public:
|
||||
|
|
@ -30,7 +33,7 @@ private:
|
|||
|
||||
void publish_scene() {
|
||||
if (object_published_) {
|
||||
return; // Избегаем повторной публикации объекта
|
||||
return;
|
||||
}
|
||||
|
||||
moveit_msgs::msg::PlanningScene planning_scene;
|
||||
|
|
@ -39,12 +42,10 @@ private:
|
|||
collision_object.header.frame_id = "world";
|
||||
collision_object.id = "table";
|
||||
|
||||
// Определяем геометрию объекта (стола)
|
||||
shape_msgs::msg::SolidPrimitive table;
|
||||
table.type = shape_msgs::msg::SolidPrimitive::BOX;
|
||||
table.dimensions = {1.2, 0.7, 0.8}; // Длина, ширина, высота
|
||||
table.dimensions = {1.2, 0.7, 0.8};
|
||||
|
||||
// Задаем позу объекта
|
||||
geometry_msgs::msg::Pose table_pose;
|
||||
table_pose.position.x = 0.0;
|
||||
table_pose.position.y = 0.0;
|
||||
|
|
@ -56,42 +57,30 @@ private:
|
|||
|
||||
planning_scene.world.collision_objects.push_back(collision_object);
|
||||
|
||||
// Добавляем объект меша
|
||||
moveit_msgs::msg::CollisionObject mesh_object;
|
||||
mesh_object.header.frame_id = "world";
|
||||
mesh_object.id = "mesh_object";
|
||||
|
||||
shape_msgs::msg::Mesh mesh;
|
||||
shapes::Mesh* m = shapes::createMeshFromResource("package://rbs_mill_assist/assets/laser/meshes/laser.dae");
|
||||
shapes::ShapeMsg mesh_msg;
|
||||
shapes::constructMsgFromShape(m, mesh_msg);
|
||||
mesh = boost::get<shape_msgs::msg::Mesh>(mesh_msg);
|
||||
|
||||
//0.30 0.0 0 0 0 3.14159
|
||||
geometry_msgs::msg::Pose mesh_pose;
|
||||
mesh_pose.position.x = 0.30;
|
||||
mesh_pose.position.y = 0.0;
|
||||
mesh_pose.position.z = 0.0;
|
||||
|
||||
mesh_pose.orientation.x = 0.0;
|
||||
mesh_pose.orientation.y = 0.0;
|
||||
mesh_pose.orientation.z = 1.0;
|
||||
mesh_pose.orientation.w = 0.0;
|
||||
|
||||
moveit_msgs::msg::ObjectColor color;
|
||||
color.id = mesh_object.id;
|
||||
color.color.r = 0.0;
|
||||
color.color.g = 1.0;
|
||||
color.color.b = 0.0;
|
||||
color.color.a = 1.0;
|
||||
std::vector<std::string> meshes = {
|
||||
"package://rbs_mill_assist/assets/laser/meshes/laser.dae",
|
||||
"package://rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL",
|
||||
"package://rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL"
|
||||
};
|
||||
|
||||
std::vector<std::vector<double>> mesh_poses = {
|
||||
{0.350, -0.170, 0.0, 0.0, 0.0, 1.0, 0.0},
|
||||
{-0.050, 0.250, 0.0, 0.0, 0.0, 0.707, 0.707},
|
||||
{-0.470, 0.250, 0.0, 0.0, 0.0, 0.707, 0.707},
|
||||
};
|
||||
|
||||
for (size_t i=0; i < meshes.size(); i++) {
|
||||
auto mesh = getMeshCollisionObject(meshes[i]);
|
||||
auto mesh_pose = arrayToPose(mesh_poses[i]);
|
||||
mesh_object.meshes.push_back(mesh);
|
||||
mesh_object.mesh_poses.push_back(mesh_pose);
|
||||
mesh_object.operation = moveit_msgs::msg::CollisionObject::ADD;
|
||||
}
|
||||
|
||||
// planning_scene.object_colors.push_back(color);
|
||||
// planning_scene.object_colors.clear();
|
||||
mesh_object.operation = moveit_msgs::msg::CollisionObject::ADD;
|
||||
|
||||
planning_scene.world.collision_objects.push_back(mesh_object);
|
||||
planning_scene.is_diff = true;
|
||||
|
|
@ -99,7 +88,32 @@ private:
|
|||
planning_scene_publisher_->publish(planning_scene);
|
||||
RCLCPP_INFO(this->get_logger(), "Published collision objects: table and mesh");
|
||||
|
||||
object_published_ = true; // Устанавливаем флаг, чтобы больше не публиковать объект
|
||||
object_published_ = true;
|
||||
}
|
||||
|
||||
shape_msgs::msg::Mesh getMeshCollisionObject(const std::string &mesh_filepath) {
|
||||
shape_msgs::msg::Mesh mesh;
|
||||
shapes::Mesh* m = shapes::createMeshFromResource(mesh_filepath);
|
||||
shapes::ShapeMsg mesh_msg;
|
||||
shapes::constructMsgFromShape(m, mesh_msg);
|
||||
mesh = boost::get<shape_msgs::msg::Mesh>(mesh_msg);
|
||||
return mesh;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Pose arrayToPose(const std::vector<double> arr) {
|
||||
if (arr.size() != 7) {
|
||||
throw std::invalid_argument("Size of array must be equal 7");
|
||||
}
|
||||
geometry_msgs::msg::Pose pose;
|
||||
pose.position.x = arr[0];
|
||||
pose.position.y = arr[1];
|
||||
pose.position.z = arr[2];
|
||||
|
||||
pose.orientation.x = arr[3];
|
||||
pose.orientation.y = arr[4];
|
||||
pose.orientation.z = arr[5];
|
||||
pose.orientation.w = arr[6];
|
||||
return pose;
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@
|
|||
<xacro:arg name="namespace" default="arm0"/>
|
||||
|
||||
<xacro:arg name="x" default="-0.20" />
|
||||
<xacro:arg name="y" default="0.0" />
|
||||
<xacro:arg name="y" default="-0.130" />
|
||||
<xacro:arg name="z" default="0.0" />
|
||||
|
||||
<xacro:arg name="roll" default="0.0" />
|
||||
|
|
|
|||
|
|
@ -386,7 +386,7 @@
|
|||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0012 0 0.10155"
|
||||
rpy="0 0 0" />
|
||||
rpy="0 0 3.14" />
|
||||
<parent
|
||||
link="Link5" />
|
||||
<child
|
||||
|
|
@ -403,17 +403,17 @@
|
|||
name="Link7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="8.67361737988404E-18 0 -0.0036061383699445"
|
||||
xyz="-1.01394587170844E-15 -5.55111512312578E-17 -0.0049950359465929"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0198935071603267" />
|
||||
value="0.0119295697834755" />
|
||||
<inertia
|
||||
ixx="1.01905343575873E-06"
|
||||
ixy="1.40555551021685E-36"
|
||||
ixz="-7.39766847562968E-22"
|
||||
iyy="1.01905343575873E-06"
|
||||
iyz="-2.63361925044231E-22"
|
||||
izz="1.43041044988815E-06" />
|
||||
ixx="4.27745590342647E-07"
|
||||
ixy="2.68928440312099E-33"
|
||||
ixz="1.44687693702654E-21"
|
||||
iyy="4.2774559034265E-07"
|
||||
iyz="3.0329437472821E-21"
|
||||
izz="3.81314853129931E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
|
|
@ -443,7 +443,7 @@
|
|||
name="Right_point"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="0.015 0.195 0.05"
|
||||
xyz="0 -0.19 0.04"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="Link6" />
|
||||
|
|
@ -456,17 +456,17 @@
|
|||
name="Link8">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.04083408558608E-17 0 -0.0036061383699445"
|
||||
xyz="-6.10622663543836E-16 -1.11022302462516E-16 -0.0049950359465929"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0198935071603267" />
|
||||
value="0.0119295697834755" />
|
||||
<inertia
|
||||
ixx="1.01905343575873E-06"
|
||||
ixy="1.10608362843633E-36"
|
||||
ixz="-7.54785908468325E-22"
|
||||
iyy="1.01905343575873E-06"
|
||||
iyz="-2.63361925044231E-22"
|
||||
izz="1.43041044988815E-06" />
|
||||
ixx="4.2774559034265E-07"
|
||||
ixy="-2.75001183405761E-33"
|
||||
ixz="3.1360121197781E-21"
|
||||
iyy="4.27745590342647E-07"
|
||||
iyz="-5.45683760877208E-22"
|
||||
izz="3.81314853129931E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
|
|
@ -496,7 +496,7 @@
|
|||
name="Left_point"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="-0.015 0.195 0.05"
|
||||
xyz="0 -0.164999999999954 0.0399999999999993"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="Link6" />
|
||||
|
|
@ -510,7 +510,7 @@
|
|||
name="grasp_point_joint"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="0.0 0.195 0.05"
|
||||
xyz="0.0 -0.177 0.04"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="Link6" />
|
||||
|
|
@ -611,7 +611,7 @@
|
|||
<xacro:gazebo_reference joint_name="left_contact_panel_joint" />
|
||||
|
||||
|
||||
<xacro:joint_hardware joint_name="ax1" hardware="${hardware}" initial_joint_position="0.0" />
|
||||
<xacro:joint_hardware joint_name="ax1" hardware="${hardware}" initial_joint_position="3.14" />
|
||||
<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" />
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@
|
|||
</plugin>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
|
|
@ -56,45 +56,51 @@
|
|||
|
||||
<include>
|
||||
<name>shildik_0</name>
|
||||
<pose>-0.5 -0.2 0.02 0 0 1.57</pose>
|
||||
<pose>-0.403959 0.250 0.02 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
<include>
|
||||
<name>shildik_1</name>
|
||||
<pose>-0.5 -0.2 0.04 0 0 1.57</pose>
|
||||
<pose>-0.447918 0.250 0.02 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
<include>
|
||||
<name>shildik_2</name>
|
||||
<pose>-0.5 -0.2 0.05 0 0 1.57</pose>
|
||||
<pose>-0.491997 0.250 0.02 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
<include>
|
||||
<name>shildik_3</name>
|
||||
<pose>-0.5 -0.2 0.06 0 0 1.57</pose>
|
||||
<pose>-0.536033 0.250 0.02 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
<include>
|
||||
<name>shildik_4</name>
|
||||
<pose>-0.5 -0.2 0.07 0 0 1.57</pose>
|
||||
<pose>-0.491997 0.250 0.03 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<static>true</static>
|
||||
<pose>0.30 0.0 0 0 0 3.14159</pose>
|
||||
<pose>0.350 -0.170 0 0 0 3.14159</pose>
|
||||
<uri>model://laser</uri>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<static>true</static>
|
||||
<pose>-0.505 -0.20 0.06 0.0 0.0 1.57</pose>
|
||||
<pose>-0.050 0.250 0.0 0.0 0.0 1.57</pose>
|
||||
<uri>model://bunker</uri>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<static>true</static>
|
||||
<pose>0.20 0 0.01 0 0 1.57</pose>
|
||||
<pose>-0.470 0.250 0 0 0 1.57</pose>
|
||||
<uri>model://bunker_4_slots</uri>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<static>true</static>
|
||||
<pose>0.350 -0.170 0.01 0 0 1.57</pose>
|
||||
<uri>model://conductor</uri>
|
||||
</include>
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue