Skip to content

Commit 29fdadb

Browse files
jslee02jslee02
committed
Add example for skeletons with spring-damper
1 parent 987c962 commit 29fdadb

File tree

5 files changed

+203
-2
lines changed

5 files changed

+203
-2
lines changed

‎examples/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ add_subdirectory(atlas_simbicon)
1212
add_subdirectory(biped_stand)
1313
add_subdirectory(box_stacking)
1414
add_subdirectory(boxes)
15+
add_subdirectory(virtual_spring_damper)
1516
add_subdirectory(drag_and_drop)
1617
add_subdirectory(empty)
1718
add_subdirectory(heightmap)

‎examples/virtual_spring_damper/CMakeLists.txt‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
cmake_minimum_required(VERSION 3.22.1)
2+
3+
get_filename_component(example_name ${CMAKE_CURRENT_LIST_DIR} NAME)
4+
5+
project(${example_name})
6+
7+
set(required_components gui-osg)
8+
set(required_libraries dart dart-gui-osg)
9+
10+
if(DART_IN_SOURCE_BUILD)
11+
dart_build_example_in_source(${example_name} LINK_LIBRARIES ${required_libraries})
12+
return()
13+
endif()
14+
15+
find_package(DART 6.15.0 REQUIRED COMPONENTS ${required_components} CONFIG)
16+
17+
file(GLOB srcs "*.cpp" "*.hpp")
18+
add_executable(${example_name} ${srcs})
19+
target_link_libraries(${example_name} PUBLIC ${required_libraries})

‎examples/virtual_spring_damper/README.md‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
This project is dependent on DART. Please make sure a proper version of DART is installed before building this project.
2+
3+
## Build Instructions
4+
5+
From this directory:
6+
7+
$ mkdir build
8+
$ cd build
9+
$ cmake ..
10+
$ make
11+
12+
## Execute Instructions
13+
14+
Launch the executable from the build directory above:
15+
16+
$ ./{generated_executable}
17+
18+
Follow the instructions detailed in the console.

‎examples/virtual_spring_damper/main.cpp‎

Lines changed: 157 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,157 @@
1+
/*
2+
* Copyright (c) 2011-2025, The DART development contributors
3+
* All rights reserved.
4+
*
5+
* The list of contributors can be found at:
6+
* https://github.com/dartsim/dart/blob/main/LICENSE
7+
*
8+
* This file is provided under the following "BSD-style" License:
9+
* Redistribution and use in source and binary forms, with or
10+
* without modification, are permitted provided that the following
11+
* conditions are met:
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19+
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20+
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21+
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22+
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23+
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24+
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25+
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26+
* USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*/
32+
33+
#include <dart/gui/osg/osg.hpp>
34+
35+
#include <dart/dart.hpp>
36+
37+
using namespace dart;
38+
using namespace dart::dynamics;
39+
using namespace dart::simulation;
40+
41+
class SpringDamperNode : public gui::osg::RealTimeWorldNode
42+
{
43+
public:
44+
SpringDamperNode(const WorldPtr& world, BodyNode* bodyA, BodyNode* bodyB)
45+
: gui::osg::RealTimeWorldNode(world), mBodyA(bodyA), mBodyB(bodyB)
46+
{
47+
mRestTransform = mBodyA->getTransform(mBodyB);
48+
}
49+
50+
void customPreStep() override
51+
{
52+
Eigen::Isometry3d T = mBodyA->getTransform(mBodyB);
53+
Eigen::Vector3d dx = T.translation() - mRestTransform.translation();
54+
Eigen::Matrix3d Rerror = mRestTransform.linear().transpose() * T.linear();
55+
Eigen::Vector3d rotVec = math::logMap(Rerror);
56+
57+
Eigen::Vector6d vel = mBodyA->getSpatialVelocity(mBodyB, mBodyB);
58+
Eigen::Vector3d relAngVel = vel.head<3>();
59+
Eigen::Vector3d relLinVel = vel.tail<3>();
60+
61+
Eigen::Vector3d forceB = -mLinearK * dx - mLinearD * relLinVel;
62+
Eigen::Vector3d torqueB = -mAngularK * rotVec - mAngularD * relAngVel;
63+
64+
Eigen::Matrix3d RB = mBodyB->getWorldTransform().linear();
65+
Eigen::Vector3d forceW = RB * forceB;
66+
Eigen::Vector3d torqueW = RB * torqueB;
67+
68+
mBodyA->addExtForce(forceW);
69+
mBodyB->addExtForce(-forceW);
70+
mBodyA->addExtTorque(torqueW);
71+
mBodyB->addExtTorque(-torqueW);
72+
}
73+
74+
private:
75+
BodyNode* mBodyA;
76+
BodyNode* mBodyB;
77+
Eigen::Isometry3d mRestTransform;
78+
double mLinearK = 1000.0;
79+
double mLinearD = 50.0;
80+
double mAngularK = 200.0;
81+
double mAngularD = 10.0;
82+
};
83+
84+
static SkeletonPtr createBox(
85+
const std::string& name,
86+
const Eigen::Vector3d& size,
87+
const Eigen::Vector3d& color,
88+
const Eigen::Vector3d& position)
89+
{
90+
SkeletonPtr skel = Skeleton::create(name);
91+
auto pair = skel->createJointAndBodyNodePair<FreeJoint>();
92+
auto body = pair.second;
93+
94+
auto shape = std::make_shared<BoxShape>(size);
95+
auto sn = body->createShapeNodeWith<
96+
VisualAspect,
97+
CollisionAspect,
98+
DynamicsAspect>(shape);
99+
sn->getVisualAspect()->setColor(color);
100+
body->setInertia(
101+
Inertia(1.0, Eigen::Vector3d::Zero(), shape->computeInertia(1.0)));
102+
103+
Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
104+
tf.translation() = position;
105+
pair.first->setTransformFromParentBodyNode(tf);
106+
107+
return skel;
108+
}
109+
110+
int main()
111+
{
112+
auto world = World::create();
113+
world->setGravity(Eigen::Vector3d(0, 0, -9.81));
114+
115+
auto boxA = createBox(
116+
"boxA",
117+
Eigen::Vector3d(0.2, 0.2, 0.2),
118+
Color::Red(),
119+
Eigen::Vector3d(-0.3, 0, 0.5));
120+
auto boxB = createBox(
121+
"boxB",
122+
Eigen::Vector3d(0.2, 0.2, 0.2),
123+
Color::Blue(),
124+
Eigen::Vector3d(0.3, 0, 0.5));
125+
126+
auto ground = Skeleton::create("ground");
127+
auto groundBody = ground->createJointAndBodyNodePair<WeldJoint>().second;
128+
auto groundShape = std::make_shared<BoxShape>(Eigen::Vector3d(5.0, 5.0, 0.1));
129+
auto groundSN = groundBody->createShapeNodeWith<
130+
VisualAspect,
131+
CollisionAspect,
132+
DynamicsAspect>(groundShape);
133+
groundSN->getVisualAspect()->setColor(Color::LightGray());
134+
135+
world->addSkeleton(boxA);
136+
world->addSkeleton(boxB);
137+
world->addSkeleton(ground);
138+
139+
auto node
140+
= new SpringDamperNode(world, boxA->getBodyNode(0), boxB->getBodyNode(0));
141+
142+
gui::osg::Viewer viewer;
143+
viewer.addWorldNode(node);
144+
viewer.addInstructionText("Press space to start simulation.\n");
145+
std::cout << viewer.getInstructions() << std::endl;
146+
147+
viewer.setUpViewInWindow(0, 0, 640, 480);
148+
viewer.getCameraManipulator()->setHomePosition(
149+
::osg::Vec3(2.0f, 2.0f, 1.5f),
150+
::osg::Vec3(0.0f, 0.0f, 0.5f),
151+
::osg::Vec3(-0.2f, -0.3f, 0.94f));
152+
viewer.setCameraManipulator(viewer.getCameraManipulator());
153+
154+
viewer.run();
155+
156+
return 0;
157+
}

‎pixi.toml‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -52,9 +52,7 @@ config = { cmd = """
5252
-S . \
5353
-B build/$PIXI_ENVIRONMENT_NAME/cpp/$BUILD_TYPE \
5454
-DCMAKE_BUILD_TYPE=$BUILD_TYPE \
55-
-DCMAKE_BUILD_WITH_INSTALL_RPATH=ON \
5655
-DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX \
57-
-DCMAKE_INSTALL_RPATH=$CONDA_PREFIX/lib \
5856
-DCMAKE_PREFIX_PATH=$CONDA_PREFIX \
5957
-DDART_BUILD_PROFILE=ON \
6058
-DDART_USE_SYSTEM_GOOGLEBENCHMARK=ON \
@@ -162,6 +160,14 @@ ex-hello-world = { cmd = """
162160
&& ./build/$PIXI_ENVIRONMENT_NAME/cpp/$BUILD_TYPE/bin/hello_world
163161
""", depends-on = ["config"], env = { BUILD_TYPE = "Release" } }
164162

163+
ex-virtual-spring-damper = { cmd = """
164+
cmake \
165+
--build build/$PIXI_ENVIRONMENT_NAME/cpp/$BUILD_TYPE \
166+
--target virtual_spring_damper \
167+
--parallel \
168+
&& ./build/$PIXI_ENVIRONMENT_NAME/cpp/$BUILD_TYPE/bin/virtual_spring_damper
169+
""", depends-on = ["config"], env = { BUILD_TYPE = "Release" } }
170+
165171
bm = { cmd = """
166172
cmake \
167173
--build build/$PIXI_ENVIRONMENT_NAME/cpp/$BUILD_TYPE \

0 commit comments

Comments
 (0)