Skip to content

Commit d604919

Browse files
iche033iche033
authored
bullet-featherstone: Add Kinematic feature (#618)
Signed-off-by: Ian Chen <ichen@openrobotics.org>
1 parent b6112c8 commit d604919

7 files changed

Lines changed: 574 additions & 0 deletions

File tree

‎bullet-featherstone/src/KinematicLinkFeatures.cc‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
/*
2+
* Copyright (C) 2024 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include "KinematicLinkFeatures.hh"
19+
20+
namespace gz {
21+
namespace physics {
22+
namespace bullet_featherstone {
23+
24+
#if BT_BULLET_VERSION >= 307
25+
/////////////////////////////////////////////////
26+
void KinematicLinkFeatures::SetLinkKinematic(
27+
const Identity &_id, bool _kinematic)
28+
{
29+
auto *link = this->ReferenceInterface<LinkInfo>(_id);
30+
auto *model = this->ReferenceInterface<ModelInfo>(link->model);
31+
32+
int collisionFlags = _kinematic ? btCollisionObject::CF_KINEMATIC_OBJECT :
33+
btCollisionObject::CF_DYNAMIC_OBJECT;
34+
35+
if (link->indexInModel.has_value())
36+
{
37+
model->body->setLinkDynamicType(link->indexInModel.value(), collisionFlags);
38+
}
39+
else
40+
{
41+
model->body->setBaseDynamicType(collisionFlags);
42+
}
43+
}
44+
45+
/////////////////////////////////////////////////
46+
bool KinematicLinkFeatures::GetLinkKinematic(const Identity &_id) const
47+
{
48+
auto *link = this->ReferenceInterface<LinkInfo>(_id);
49+
auto *model = this->ReferenceInterface<ModelInfo>(link->model);
50+
51+
int indexInModel = link->indexInModel.value_or(-1);
52+
return model->body->isLinkKinematic(indexInModel);
53+
}
54+
#endif
55+
56+
} // namespace bullet_featherstone
57+
} // namespace physics
58+
} // namespace gz

‎bullet-featherstone/src/KinematicLinkFeatures.hh‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
/*
2+
* Copyright (C) 2024 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#ifndef GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_KINEMATICLINKFEATURES_HH_
19+
#define GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_KINEMATICLINKFEATURES_HH_
20+
21+
#include <gz/physics/KinematicLink.hh>
22+
23+
#include "Base.hh"
24+
25+
namespace gz {
26+
namespace physics {
27+
namespace bullet_featherstone {
28+
29+
#if BT_BULLET_VERSION >= 307
30+
struct KinematicLinkFeatureList : FeatureList<
31+
KinematicLink
32+
> { };
33+
34+
class KinematicLinkFeatures :
35+
public virtual Base,
36+
public virtual Implements3d<KinematicLinkFeatureList>
37+
{
38+
// ----- Set / Get Kinematic -----
39+
public: void SetLinkKinematic(
40+
const Identity &_id,
41+
bool _kinematic) override;
42+
43+
public: bool GetLinkKinematic(
44+
const Identity &_id) const override;
45+
};
46+
#endif
47+
48+
} // namespace bullet_featherstone
49+
} // namespace physics
50+
} // namespace gz
51+
52+
#endif

‎bullet-featherstone/src/SDFFeatures.cc‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -924,6 +924,18 @@ Identity SDFFeatures::ConstructSdfModelImpl(
924924
this->AddSdfCollision(linkID, *linkSdf->CollisionByIndex(c), isStatic);
925925
}
926926
}
927+
928+
#if BT_BULLET_VERSION >= 307
929+
// Set kinematic mode
930+
// Do this after adding collisions
931+
if (linkSdf->Kinematic())
932+
{
933+
auto *linkInfo = this->ReferenceInterface<LinkInfo>(linkID);
934+
int indexInModel = linkInfo->indexInModel.value_or(-1);
935+
model->body->setLinkDynamicType(indexInModel,
936+
btCollisionObject::CF_KINEMATIC_OBJECT);
937+
}
938+
#endif
927939
}
928940

929941
// Add the remaining links in the model without constructing the bullet

‎bullet-featherstone/src/plugin.cc‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include "FreeGroupFeatures.hh"
2626
#include "ShapeFeatures.hh"
2727
#include "JointFeatures.hh"
28+
#include "KinematicLinkFeatures.hh"
2829
#include "KinematicsFeatures.hh"
2930
#include "LinkFeatures.hh"
3031
#include "SDFFeatures.hh"
@@ -39,6 +40,9 @@ struct BulletFeatures : FeatureList <
3940
EntityManagementFeatureList,
4041
SimulationFeatureList,
4142
FreeGroupFeatureList,
43+
#if BT_BULLET_VERSION >= 307
44+
KinematicLinkFeatureList,
45+
#endif
4246
KinematicsFeatureList,
4347
LinkFeatureList,
4448
SDFFeatureList,
@@ -53,6 +57,9 @@ class Plugin :
5357
public virtual EntityManagementFeatures,
5458
public virtual SimulationFeatures,
5559
public virtual FreeGroupFeatures,
60+
#if BT_BULLET_VERSION >= 307
61+
public virtual KinematicLinkFeatures,
62+
#endif
5663
public virtual KinematicsFeatures,
5764
public virtual LinkFeatures,
5865
public virtual SDFFeatures,

‎include/gz/physics/KinematicLink.hh‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
/*
2+
* Copyright (C) 2024 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#ifndef GZ_PHYSICS_KINEMATICLINK_HH_
19+
#define GZ_PHYSICS_KINEMATICLINK_HH_
20+
21+
#include <gz/physics/FeatureList.hh>
22+
23+
namespace gz
24+
{
25+
namespace physics
26+
{
27+
/////////////////////////////////////////////////
28+
class GZ_PHYSICS_VISIBLE KinematicLink
29+
: public virtual Feature
30+
{
31+
/// \brief The Link API for setting link to be kinematic
32+
public: template <typename PolicyT, typename FeaturesT>
33+
class Link : public virtual Feature::Link<PolicyT, FeaturesT>
34+
{
35+
/// \brief Set link to be kinematic.
36+
/// A kinematic link does not react to forces, e.g. gravity or other
37+
/// dynamic objects. It reacts to pose or velocity commands that are
38+
/// set on the link (via a FreeGroup) or on the joint that connects the
39+
/// kinematic links.
40+
/// \param[i] _kinematic True to make this link kinematic.
41+
public: void SetKinematic(bool _kinematic);
42+
43+
/// \brief Get whether this link is kinematic.
44+
/// \return True if the link is kinematic, false otherwise.
45+
/// \sa SetKinematic
46+
public: bool GetKinematic() const;
47+
};
48+
49+
public: template <typename PolicyT>
50+
class Implementation : public virtual Feature::Implementation<PolicyT>
51+
{
52+
/// \brief Implementation API for setting a link to be kinematic
53+
/// \param[in] _id Identity of the link
54+
/// \param[in] _kinematic True to make this link kinematic
55+
public: virtual void SetLinkKinematic(
56+
const Identity &_shapeID, bool _kinematic) = 0;
57+
58+
/// \brief Implementation API for getting whether a link is kinematic
59+
/// \param[in] _id Identity of the link
60+
/// \return True if the link is kinematic, false otherwise.
61+
public: virtual bool GetLinkKinematic(
62+
const Identity &_shapeID) const = 0;
63+
};
64+
};
65+
}
66+
}
67+
68+
#include <gz/physics/detail/KinematicLink.hh>
69+
70+
#endif

‎include/gz/physics/detail/KinematicLink.hh‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
/*
2+
* Copyright (C) 2024 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#ifndef GZ_PHYSICS_DETAIL_KINEMATICLINK_HH_
19+
#define GZ_PHYSICS_DETAIL_KINEMATICLINK_HH_
20+
21+
#include <gz/physics/KinematicLink.hh>
22+
#include <gz/physics/FeatureList.hh>
23+
24+
namespace gz
25+
{
26+
namespace physics
27+
{
28+
/////////////////////////////////////////////////
29+
template <typename PolicyT, typename FeaturesT>
30+
void KinematicLink::Link<PolicyT, FeaturesT>::SetKinematic(bool _kinematic)
31+
{
32+
this->template Interface<KinematicLink>()
33+
->SetLinkKinematic(this->identity, _kinematic);
34+
}
35+
36+
/////////////////////////////////////////////////
37+
template <typename PolicyT, typename FeaturesT>
38+
bool KinematicLink::Link<PolicyT, FeaturesT>::GetKinematic() const
39+
{
40+
return this->template Interface<KinematicLink>()
41+
->GetLinkKinematic(this->identity);
42+
}
43+
} // namespace physics
44+
} // namespace gz
45+
46+
#endif

0 commit comments

Comments
 (0)