Skip to content

Commit f922785

Browse files
jfernandez37jfernandez37
committed
Updated vacuum models for reduced stickiness. Update agv tray interface plugin to correclty publish status.
2 parents 52e4344 + 7cf8606 commit f922785

5 files changed

Lines changed: 129 additions & 86 deletions

File tree

‎ariac_gz/models/vacuum_tools/vg_2/model.sdf‎

Lines changed: 38 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,42 @@
2525

2626
</link>
2727

28+
<link name="antistick_link1">
29+
<inertial auto="true"/>
30+
<pose> 0.025 0 -0.06305 0 0 0 </pose>
31+
<collision name="collision">
32+
<density>1</density>
33+
<geometry>
34+
<box>
35+
<size>0.01 0.01 0.001</size>
36+
</box>
37+
</geometry>
38+
</collision>
39+
</link>
40+
41+
<link name="antistick_link2">
42+
<inertial auto="true"/>
43+
<pose> -0.025 0 -0.06305 0 0 0 </pose>
44+
<collision name="collision">
45+
<density>1</density>
46+
<geometry>
47+
<box>
48+
<size>0.01 0.01 0.001</size>
49+
</box>
50+
</geometry>
51+
</collision>
52+
</link>
53+
54+
<joint name="antistick_joint1" type="fixed">
55+
<parent>base_link</parent>
56+
<child>antistick_link1</child>
57+
</joint>
58+
59+
<joint name="antistick_joint2" type="fixed">
60+
<parent>base_link</parent>
61+
<child>antistick_link2</child>
62+
</joint>
63+
2864
<link name="contact_link">
2965
<inertial auto="true"/>
3066
<pose> 0 0 -0.00505 0 0 0</pose>
@@ -112,7 +148,7 @@
112148
<xyz>0 0 1</xyz>
113149
<limit>
114150
<lower>0</lower>
115-
<upper>0.001</upper>
151+
<upper>0.004</upper>
116152
<effort>10</effort>
117153
</limit>
118154
</axis>
@@ -125,7 +161,7 @@
125161
<xyz>0 0 1</xyz>
126162
<limit>
127163
<lower>0</lower>
128-
<upper>0.001</upper>
164+
<upper>0.004</upper>
129165
<effort>10</effort>
130166
</limit>
131167
</axis>

‎ariac_gz/models/vacuum_tools/vg_4/model.sdf‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -163,7 +163,7 @@
163163
<xyz>0 0 1</xyz>
164164
<limit>
165165
<lower>0</lower>
166-
<upper>0.001</upper>
166+
<upper>0.004</upper>
167167
<effort>10</effort>
168168
</limit>
169169
</axis>
@@ -176,7 +176,7 @@
176176
<xyz>0 0 1</xyz>
177177
<limit>
178178
<lower>0</lower>
179-
<upper>0.001</upper>
179+
<upper>0.004</upper>
180180
<effort>10</effort>
181181
</limit>
182182
</axis>
@@ -189,7 +189,7 @@
189189
<xyz>0 0 1</xyz>
190190
<limit>
191191
<lower>0</lower>
192-
<upper>0.001</upper>
192+
<upper>0.004</upper>
193193
<effort>10</effort>
194194
</limit>
195195
</axis>
@@ -202,7 +202,7 @@
202202
<xyz>0 0 1</xyz>
203203
<limit>
204204
<lower>0</lower>
205-
<upper>0.001</upper>
205+
<upper>0.004</upper>
206206
<effort>10</effort>
207207
</limit>
208208
</axis>

‎ariac_plugins/include/ariac_plugins/agv_tray_interface_plugin.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -108,8 +108,8 @@ namespace ariac_plugins
108108
std::optional<ariac_components::Kit> kit_component = std::nullopt;
109109
std::mutex kit_mutex_;
110110

111-
gz::math::Pose3d tray_transform = gz::math::Pose3d(0.0, 0.0, 0.35, 0.0, 0.0, 1.57);
112-
gz::math::Pose3d tray_spawn_transform = gz::math::Pose3d(0.0, 0.0, 0.3501, 0.0, 0.0, 1.57);
111+
gz::math::Pose3d tray_transform = gz::math::Pose3d(0.0, 0.0, 0.35, 0.0, 0.0, -1.57);
112+
gz::math::Pose3d tray_spawn_transform = gz::math::Pose3d(0.0, 0.0, 0.3501, 0.0, 0.0, -1.57);
113113

114114
// ROS
115115
rclcpp::Node::SharedPtr ros_node;

‎ariac_plugins/src/top_shell_plugin.cpp‎

Lines changed: 78 additions & 76 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,84 @@ void TopShellPlugin::PreUpdate(
4242
const gz::sim::UpdateInfo &_info,
4343
gz::sim::EntityComponentManager &_ecm)
4444
{
45+
if(teleport_state != TopShellTeleportState::FINISHED){
46+
switch(teleport_state){
47+
case TopShellTeleportState::IDLE:
48+
break;
49+
case TopShellTeleportState::READY:
50+
{
51+
if (bottom_shell_entity == gz::sim::kNullEntity){
52+
break;
53+
}
54+
55+
gz::sim::Entity bottom_shell_has_parent = false;
56+
_ecm.Each<gz::sim::components::DetachableJoint>(
57+
[&](
58+
const gz::sim::Entity &entity,
59+
const gz::sim::components::DetachableJoint *detachable_joint
60+
) -> bool {
61+
if(detachable_joint->Data().childLink == gz::sim::Model(bottom_shell_entity).LinkByName(_ecm, "base_link")){
62+
bottom_shell_has_parent = true;
63+
return false;
64+
}
65+
return true;
66+
}
67+
);
68+
69+
if(bottom_shell_has_parent){
70+
break;
71+
}
72+
73+
std::optional<gz::math::Pose3d> bottom_shell_pose = gz::sim::Link(gz::sim::Model(bottom_shell_entity).LinkByName(_ecm, "base_link")).WorldPose(_ecm);
74+
75+
if(!bottom_shell_pose.has_value()){
76+
break;
77+
}
78+
79+
gz::math::Pose3d target_pose = flipped_bottom_shell_pose;
80+
81+
target_pose.SetX(bottom_shell_pose.value().X());
82+
target_pose.SetY(bottom_shell_pose.value().Y());
83+
target_pose.SetZ(bottom_shell_pose.value().Z());
84+
gz::sim::Model(bottom_shell_entity).SetWorldPoseCmd(_ecm, target_pose);
85+
teleport_state = TopShellTeleportState::JOINT_NEEDED;
86+
87+
teleport_step = _info.iterations;
88+
89+
break;
90+
}
91+
case TopShellTeleportState::JOINT_NEEDED:
92+
93+
lock_joint = _ecm.CreateEntity();
94+
95+
_ecm.CreateComponent(lock_joint, gz::sim::components::DetachableJoint({
96+
section_3_link_entity,
97+
gz::sim::Model(bottom_shell_entity).LinkByName(_ecm, "base_link"),
98+
"fixed"}));
99+
100+
teleport_state = TopShellTeleportState::JOINT_REMOVAL;
101+
102+
break;
103+
case TopShellTeleportState::JOINT_REMOVAL:
104+
if(_info.iterations - teleport_step < 100){
105+
break;
106+
}
107+
108+
_ecm.RequestRemoveEntity(lock_joint);
109+
110+
lock_joint = gz::sim::kNullEntity;
111+
112+
teleport_state = TopShellTeleportState::FINISHED;
113+
break;
114+
115+
case TopShellTeleportState::FINISHED:
116+
break;
117+
118+
default:
119+
break;
120+
}
121+
}
122+
45123
if(lock_state == TopShellLockState::LOCKED){
46124
return;
47125
}
@@ -79,82 +157,6 @@ void TopShellPlugin::PreUpdate(
79157

80158
lock_state = TopShellLockState::LOCKED;
81159
}
82-
83-
switch(teleport_state){
84-
case TopShellTeleportState::IDLE:
85-
break;
86-
case TopShellTeleportState::READY:
87-
{
88-
if (bottom_shell_entity == gz::sim::kNullEntity){
89-
break;
90-
}
91-
92-
gz::sim::Entity bottom_shell_has_parent = false;
93-
_ecm.Each<gz::sim::components::DetachableJoint>(
94-
[&](
95-
const gz::sim::Entity &entity,
96-
const gz::sim::components::DetachableJoint *detachable_joint
97-
) -> bool {
98-
if(detachable_joint->Data().childLink == gz::sim::Model(bottom_shell_entity).LinkByName(_ecm, "base_link")){
99-
bottom_shell_has_parent = true;
100-
return false;
101-
}
102-
return true;
103-
}
104-
);
105-
106-
if(bottom_shell_has_parent){
107-
break;
108-
}
109-
110-
std::optional<gz::math::Pose3d> bottom_shell_pose = gz::sim::Link(gz::sim::Model(bottom_shell_entity).LinkByName(_ecm, "base_link")).WorldPose(_ecm);
111-
112-
if(!bottom_shell_pose.has_value()){
113-
break;
114-
}
115-
116-
gz::math::Pose3d target_pose = flipped_bottom_shell_pose;
117-
118-
target_pose.SetX(bottom_shell_pose.value().X());
119-
target_pose.SetY(bottom_shell_pose.value().Y());
120-
target_pose.SetZ(bottom_shell_pose.value().Z());
121-
gz::sim::Model(bottom_shell_entity).SetWorldPoseCmd(_ecm, target_pose);
122-
teleport_state = TopShellTeleportState::JOINT_NEEDED;
123-
124-
teleport_step = _info.iterations;
125-
126-
break;
127-
}
128-
case TopShellTeleportState::JOINT_NEEDED:
129-
130-
lock_joint = _ecm.CreateEntity();
131-
132-
_ecm.CreateComponent(lock_joint, gz::sim::components::DetachableJoint({
133-
section_3_link_entity,
134-
gz::sim::Model(bottom_shell_entity).LinkByName(_ecm, "base_link"),
135-
"fixed"}));
136-
137-
teleport_state = TopShellTeleportState::JOINT_REMOVAL;
138-
139-
break;
140-
case TopShellTeleportState::JOINT_REMOVAL:
141-
if(_info.iterations - teleport_step < 100){
142-
break;
143-
}
144-
145-
_ecm.RequestRemoveEntity(lock_joint);
146-
147-
lock_joint = gz::sim::kNullEntity;
148-
149-
teleport_state = TopShellTeleportState::FINISHED;
150-
break;
151-
152-
case TopShellTeleportState::FINISHED:
153-
break;
154-
155-
default:
156-
break;
157-
}
158160
}
159161

160162
gz::sim::Entity TopShellPlugin::get_base_module_shell(

‎ariac_plugins/src/vacuum_tool_plugin.cpp‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -126,6 +126,12 @@ void VacuumToolPlugin::PreUpdate(const gz::sim::UpdateInfo &,
126126
{
127127
switch (lock_state)
128128
{
129+
case VacuumToolLockState::UNLOCKED: {
130+
for(gz::sim::Joint joint : suction_cup_joints){
131+
joint.SetVelocity(_ecm, {-0.0008});
132+
}
133+
break;
134+
}
129135
case VacuumToolLockState::LOCK_REQUESTED: {
130136
// Get link entity for bottom shell
131137
auto model = _ecm.EntityByName(attach_shell_name);
@@ -337,8 +343,7 @@ bool VacuumToolPlugin::should_malfunction()
337343
return false;
338344
}
339345

340-
malfunction_active = std::find_if(
341-
malfunctions.begin(), malfunctions.end(),
346+
malfunction_active = std::find_if(malfunctions.begin(), malfunctions.end(),
342347
[this](const auto& p) { return !p.second && p.first == grasp_occurrence; }
343348
) != malfunctions.end();
344349

0 commit comments

Comments
 (0)