Skip to content

Commit 0ff0f62

Browse files
mergify[bot]iche033cagueroluca-della-vedova
authored
Add DetachableJointEnforceFixedConstraint component (#3644) (#3749)
When <enforce_fixed_constraint> (see #2984) is set in the <physics> SDF element, this setting is currently applied to all detachable joint. The new DetachableJointEnforceFixedConstraint allows user to override this global setting for each detachable joint. Note that the enforce_fixed_constraint setting requires the SetWeldChildToParent gz-physics feature which is only available in bullet-featherstone plugin. Generated-by: Gemini 3 --------- (cherry picked from commit 7579cc4) Signed-off-by: Ian Chen <iche@intrinsic.ai> Co-authored-by: Ian Chen <iche@intrinsic.ai> Co-authored-by: Carlos Agüero <caguero@honurobotics.com> Co-authored-by: Luca Della Vedova <lucadv@intrinsic.ai>
1 parent 7145a87 commit 0ff0f62

4 files changed

Lines changed: 277 additions & 1 deletion

File tree

‎include/gz/sim/components/DetachableJoint.hh‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,14 @@ namespace components
9696
serializers::DetachableJointInfoSerializer>;
9797
GZ_SIM_REGISTER_COMPONENT("gz_sim_components.DetachableJoint",
9898
DetachableJoint)
99+
100+
/// \brief A component that overrides the global enforce_fixed_constraint
101+
/// policy for a specific detachable joint.
102+
using DetachableJointEnforceFixedConstraint =
103+
Component<bool, class DetachableJointEnforceFixedConstraintTag>;
104+
GZ_SIM_REGISTER_COMPONENT(
105+
"gz_sim_components.DetachableJointEnforceFixedConstraint",
106+
DetachableJointEnforceFixedConstraint)
99107
}
100108
}
101109
}

‎src/systems/physics/Physics.cc‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2067,7 +2067,10 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
20672067
this->topLevelModelMap.insert(std::make_pair(_entity,
20682068
topLevelModel(_entity, _ecm)));
20692069

2070-
if (this->enforceFixedConstraint)
2070+
bool enforce = _ecm.ComponentData<
2071+
components::DetachableJointEnforceFixedConstraint>(
2072+
_entity).value_or(this->enforceFixedConstraint);
2073+
if (enforce)
20712074
{
20722075
auto jointPtrWeld = this->entityJointMap
20732076
.EntityCast<SetFixedJointWeldChildToParentFeatureList>(_entity);

‎test/integration/detachable_joint.cc‎

Lines changed: 127 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,9 @@
3737
#include "gz/sim/components/Model.hh"
3838
#include "gz/sim/components/Name.hh"
3939
#include "gz/sim/components/Pose.hh"
40+
#include "gz/sim/components/PoseCmd.hh"
4041
#include "gz/sim/components/WindMode.hh"
42+
#include "gz/sim/components/DetachableJoint.hh"
4143

4244
#include "../helpers/Relay.hh"
4345
#include "../helpers/EnvTestFixture.hh"
@@ -435,3 +437,128 @@ TEST_F(DetachableJointTest,
435437
// should be close.
436438
EXPECT_TRUE(abs(distTraveledB1 - distTraveledVehicle) < 0.01);
437439
}
440+
441+
/////////////////////////////////////////////////
442+
TEST_F(DetachableJointTest,
443+
GZ_UTILS_TEST_DISABLED_ON_WIN32(EnforceFixedConstraint))
444+
{
445+
this->StartServer(common::joinPaths("/test", "worlds",
446+
"detachable_joint_enforce.sdf"));
447+
448+
Entity modelM1 = kNullEntity;
449+
Entity modelM3 = kNullEntity;
450+
bool initialized = false;
451+
452+
test::Relay testSystem;
453+
testSystem.OnPreUpdate([&](const UpdateInfo &,
454+
EntityComponentManager &_ecm)
455+
{
456+
if (!initialized)
457+
{
458+
modelM1 = _ecm.EntityByComponents(
459+
components::Model(), components::Name("M1"));
460+
ASSERT_NE(kNullEntity, modelM1);
461+
modelM3 = _ecm.EntityByComponents(
462+
components::Model(), components::Name("M3"));
463+
ASSERT_NE(kNullEntity, modelM3);
464+
465+
// Locate the detachable joints for M1/M2 and M3/M4.
466+
Entity jointM1M2 = kNullEntity;
467+
Entity jointM3M4 = kNullEntity;
468+
469+
_ecm.Each<components::DetachableJoint>(
470+
[&](const Entity &_entity,
471+
const components::DetachableJoint *_joint) -> bool
472+
{
473+
auto parentComp = _ecm.Component<components::Name>(
474+
_joint->Data().parentLink);
475+
if (parentComp)
476+
{
477+
if (parentComp->Data() == "body")
478+
jointM1M2 = _entity;
479+
else if (parentComp->Data() == "body1")
480+
jointM3M4 = _entity;
481+
}
482+
return true;
483+
});
484+
485+
ASSERT_NE(kNullEntity, jointM1M2);
486+
ASSERT_NE(kNullEntity, jointM3M4);
487+
488+
// Enable enforced fixed constraint for the M1/M2 joint.
489+
_ecm.CreateComponent(
490+
jointM1M2,
491+
components::DetachableJointEnforceFixedConstraint(true));
492+
493+
// Disable enforced fixed constraint for the M3/M4 joint.
494+
_ecm.CreateComponent(
495+
jointM3M4,
496+
components::DetachableJointEnforceFixedConstraint(false));
497+
498+
initialized = true;
499+
}
500+
501+
// Teleport the parent models M1 and M3.
502+
_ecm.SetComponentData<components::WorldPoseCmd>(
503+
modelM1, math::Pose3d(0, 60, 1, 0, 0, 0));
504+
_ecm.SetComponentData<components::WorldPoseCmd>(
505+
modelM3, math::Pose3d(10, 60, 1, 0, 0, 0));
506+
});
507+
508+
std::vector<math::Pose3d> m1LinkPoses, m2LinkPoses;
509+
std::vector<math::Pose3d> m3LinkPoses, m4LinkPoses;
510+
511+
testSystem.OnPostUpdate([&](const UpdateInfo &,
512+
const EntityComponentManager &_ecm)
513+
{
514+
_ecm.Each<components::Link>(
515+
[&](const Entity &_entity, const components::Link *) -> bool
516+
{
517+
Link link(_entity);
518+
auto worldPose = link.WorldPose(_ecm);
519+
if (!worldPose)
520+
return true;
521+
522+
auto parentModel = _ecm.ParentEntity(_entity);
523+
auto parentName = _ecm.ComponentData<components::Name>(
524+
parentModel).value_or("");
525+
526+
if (parentName == "M1")
527+
m1LinkPoses.push_back(*worldPose);
528+
else if (parentName == "M2")
529+
m2LinkPoses.push_back(*worldPose);
530+
else if (parentName == "M3")
531+
m3LinkPoses.push_back(*worldPose);
532+
else if (parentName == "M4")
533+
m4LinkPoses.push_back(*worldPose);
534+
535+
return true;
536+
});
537+
});
538+
539+
this->server->AddSystem(testSystem.systemPtr);
540+
541+
const std::size_t nIters{10};
542+
this->server->Run(true, nIters, false);
543+
544+
ASSERT_EQ(nIters, m1LinkPoses.size());
545+
ASSERT_EQ(nIters, m2LinkPoses.size());
546+
ASSERT_EQ(nIters, m3LinkPoses.size());
547+
ASSERT_EQ(nIters, m4LinkPoses.size());
548+
549+
// M1/M2 has enforce fixed constraint set to true:
550+
// the distance should remain at ~2.0m because the child is moved
551+
// to maintain the rigid child-to-parent pose offset.
552+
double M1M2Dist =
553+
(m2LinkPoses.back().Pos() - m1LinkPoses.back().Pos()).Length();
554+
EXPECT_NEAR(2.0, M1M2Dist, 1e-5);
555+
556+
// M3/M4 has enforce fixed constraint set to false:
557+
// The parent-child pose offset is not strictly enforced. Their relative
558+
// distance is determined by the constraint solver.
559+
double M3M4Dist =
560+
(m4LinkPoses.back().Pos() - m3LinkPoses.back().Pos()).Length();
561+
562+
// There should be non-zero difference between M1/M2 and M3/M4 pose offsets
563+
EXPECT_GT(std::fabs(M1M2Dist - M3M4Dist), 1e-6);
564+
}

‎test/worlds/detachable_joint_enforce.sdf‎

Lines changed: 138 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,138 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.6">
3+
<world name="detachable_joint">
4+
<physics name="fast" type="ignored">
5+
<real_time_factor>0</real_time_factor>
6+
</physics>
7+
8+
<plugin filename="gz-sim-physics-system"
9+
name="gz::sim::systems::Physics">
10+
<engine>
11+
<filename>gz-physics-bullet-featherstone-plugin</filename>
12+
</engine>
13+
</plugin>
14+
15+
<model name="ground_plane">
16+
<static>true</static>
17+
<link name="link">
18+
<collision name="collision">
19+
<geometry>
20+
<plane>
21+
<normal>0 0 1</normal>
22+
<size>100 100</size>
23+
</plane>
24+
</geometry>
25+
</collision>
26+
</link>
27+
</model>
28+
29+
<model name="M1">
30+
<pose>0 0 1 0 0 0</pose>
31+
<link name="body">
32+
<inertial>
33+
<mass>1</mass>
34+
<inertia>
35+
<ixx>0.667</ixx>
36+
<ixy>0</ixy>
37+
<ixz>0</ixz>
38+
<iyy>0.667</iyy>
39+
<iyz>0</iyz>
40+
<izz>0.667</izz>
41+
</inertia>
42+
</inertial>
43+
<collision name="collision">
44+
<geometry>
45+
<box>
46+
<size>2.0 2.0 2.0</size>
47+
</box>
48+
</geometry>
49+
</collision>
50+
</link>
51+
52+
<plugin filename="gz-sim-detachable-joint-system"
53+
name="gz::sim::systems::DetachableJoint">
54+
<parent_link>body</parent_link>
55+
<child_model>M2</child_model>
56+
<child_link>body</child_link>
57+
</plugin>
58+
</model>
59+
60+
<model name="M2">
61+
<pose>0 2 1 0 0 0</pose>
62+
<link name="body">
63+
<inertial>
64+
<mass>1000</mass>
65+
<inertia>
66+
<ixx>0.667</ixx>
67+
<ixy>0</ixy>
68+
<ixz>0</ixz>
69+
<iyy>0.667</iyy>
70+
<iyz>0</iyz>
71+
<izz>0.667</izz>
72+
</inertia>
73+
</inertial>
74+
<collision name="collision">
75+
<geometry>
76+
<box>
77+
<size>2.0 2.0 2.0</size>
78+
</box>
79+
</geometry>
80+
</collision>
81+
</link>
82+
</model>
83+
84+
<model name="M3">
85+
<pose>10 0 1 0 0 0</pose>
86+
<link name="body1">
87+
<inertial>
88+
<mass>1</mass>
89+
<inertia>
90+
<ixx>0.667</ixx>
91+
<ixy>0</ixy>
92+
<ixz>0</ixz>
93+
<iyy>0.667</iyy>
94+
<iyz>0</iyz>
95+
<izz>0.667</izz>
96+
</inertia>
97+
</inertial>
98+
<collision name="collision">
99+
<geometry>
100+
<box>
101+
<size>2.0 2.0 2.0</size>
102+
</box>
103+
</geometry>
104+
</collision>
105+
</link>
106+
<plugin filename="gz-sim-detachable-joint-system"
107+
name="gz::sim::systems::DetachableJoint">
108+
<parent_link>body1</parent_link>
109+
<child_model>M4</child_model>
110+
<child_link>body2</child_link>
111+
</plugin>
112+
</model>
113+
114+
<model name="M4">
115+
<pose>10 2 1 0 0 0</pose>
116+
<link name="body2">
117+
<inertial>
118+
<mass>1000</mass>
119+
<inertia>
120+
<ixx>0.667</ixx>
121+
<ixy>0</ixy>
122+
<ixz>0</ixz>
123+
<iyy>0.667</iyy>
124+
<iyz>0</iyz>
125+
<izz>0.667</izz>
126+
</inertia>
127+
</inertial>
128+
<collision name="collision">
129+
<geometry>
130+
<box>
131+
<size>2.0 2.0 2.0</size>
132+
</box>
133+
</geometry>
134+
</collision>
135+
</link>
136+
</model>
137+
</world>
138+
</sdf>

0 commit comments

Comments
 (0)