Skip to content

Commit 11fec94

Browse files
jfernandez37jfernandez37
committed
Added fixes to the vacuum tool malfunction challenge and the suction cups not being in contact issue
1 parent e44e678 commit 11fec94

4 files changed

Lines changed: 91 additions & 84 deletions

File tree

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -112,7 +112,7 @@
112112
<xyz>0 0 1</xyz>
113113
<limit>
114114
<lower>0</lower>
115-
<upper>0.001</upper>
115+
<upper>0.004</upper>
116116
<effort>10</effort>
117117
</limit>
118118
</axis>
@@ -125,7 +125,7 @@
125125
<xyz>0 0 1</xyz>
126126
<limit>
127127
<lower>0</lower>
128-
<upper>0.001</upper>
128+
<upper>0.004</upper>
129129
<effort>10</effort>
130130
</limit>
131131
</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/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)