1 /*
2 * Copyright (c) 2011-2021, 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/master/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/dart.hpp>
34 #include <dart/gui/osg/osg.hpp>
35 #include <dart/utils/utils.hpp>
36
37 using namespace dart;
38
39 //==============================================================================
40 class CustomWorldNode : public dart::gui::osg::RealTimeWorldNode
41 {
42 public:
CustomWorldNode(dart::simulation::WorldPtr world,dart::dynamics::SkeletonPtr biped,::osg::ref_ptr<osgShadow::ShadowTechnique> shadow=nullptr)43 CustomWorldNode(
44 dart::simulation::WorldPtr world,
45 dart::dynamics::SkeletonPtr biped,
46 ::osg::ref_ptr<osgShadow::ShadowTechnique> shadow = nullptr)
47 : dart::gui::osg::RealTimeWorldNode(std::move(world), std::move(shadow)),
48 mBiped(std::move(biped))
49 {
50 mLeftHeel = mBiped->getBodyNode("h_heel_left");
51
52 mLeftFoot[0] = mBiped->getDof("j_heel_left_1")->getIndexInSkeleton();
53 mLeftFoot[1] = mBiped->getDof("j_toe_left")->getIndexInSkeleton();
54
55 mRightFoot[0] = mBiped->getDof("j_heel_right_1")->getIndexInSkeleton();
56 mRightFoot[1] = mBiped->getDof("j_toe_right")->getIndexInSkeleton();
57
58 mTimestep = mWorld->getTimeStep();
59 mFrame = 0;
60 const int nDof = static_cast<int>(mBiped->getNumDofs());
61 mKp = Eigen::MatrixXd::Identity(nDof, nDof);
62 mKd = Eigen::MatrixXd::Identity(nDof, nDof);
63
64 mTorques.resize(nDof);
65 mTorques.setZero();
66
67 mDesiredDofs = mBiped->getPositions();
68
69 // using SPD results in simple Kp coefficients
70 for (int i = 0; i < 6; i++)
71 {
72 mKp(i, i) = 0.0;
73 mKd(i, i) = 0.0;
74 }
75 for (int i = 6; i < nDof; i++)
76 mKp(i, i) = 400.0;
77 for (int i = 6; i < nDof; i++)
78 mKd(i, i) = 40.0;
79
80 mPreOffset = 0.0;
81 }
82
customPreRefresh()83 void customPreRefresh()
84 {
85 // Use this function to execute custom code before each time that the
86 // window is rendered. This function can be deleted if it does not need
87 // to be used.
88 }
89
customPostRefresh()90 void customPostRefresh()
91 {
92 // Use this function to execute custom code after each time that the
93 // window is rendered. This function can be deleted if it does not need
94 // to be used.
95 }
96
customPreStep()97 void customPreStep()
98 {
99 const Eigen::VectorXd dof = mBiped->getPositions();
100 const Eigen::VectorXd dofVel = mBiped->getVelocities();
101 const Eigen::VectorXd constrForces = mBiped->getConstraintForces();
102
103 // SPD tracking
104 // std::size_t nDof = mSkel->getNumDofs();
105 const Eigen::MatrixXd invM
106 = (mBiped->getMassMatrix() + mKd * mTimestep).inverse();
107 const Eigen::VectorXd p = -mKp * (dof + dofVel * mTimestep - mDesiredDofs);
108 const Eigen::VectorXd d = -mKd * dofVel;
109 const Eigen::VectorXd qddot
110 = invM
111 * (-mBiped->getCoriolisAndGravityForces() + p + d + constrForces);
112
113 mTorques = p + d - mKd * qddot * mTimestep;
114
115 // ankle strategy for sagital plane
116 const Eigen::Vector3d com = mBiped->getCOM();
117 const Eigen::Vector3d cop
118 = mLeftHeel->getTransform() * Eigen::Vector3d(0.05, 0, 0);
119
120 double offset = com[0] - cop[0];
121 if (offset < 0.1 && offset > 0.0)
122 {
123 double k1 = 200.0;
124 double k2 = 100.0;
125 double kd = 10.0;
126 mTorques[mLeftFoot[0]] += -k1 * offset + kd * (mPreOffset - offset);
127 mTorques[mLeftFoot[1]] += -k2 * offset + kd * (mPreOffset - offset);
128 mTorques[mRightFoot[0]] += -k1 * offset + kd * (mPreOffset - offset);
129 mTorques[mRightFoot[1]] += -k2 * offset + kd * (mPreOffset - offset);
130 mPreOffset = offset;
131 }
132 else if (offset > -0.2 && offset < -0.05)
133 {
134 double k1 = 2000.0;
135 double k2 = 100.0;
136 double kd = 100.0;
137 mTorques[mLeftFoot[0]] += -k1 * offset + kd * (mPreOffset - offset);
138 mTorques[mLeftFoot[1]] += -k2 * offset + kd * (mPreOffset - offset);
139 mTorques[mRightFoot[0]] += -k1 * offset + kd * (mPreOffset - offset);
140 mTorques[mRightFoot[1]] += -k2 * offset + kd * (mPreOffset - offset);
141 mPreOffset = offset;
142 }
143
144 // Just to make sure no illegal torque is used
145 for (int i = 0; i < 6; i++)
146 {
147 mTorques[i] = 0.0;
148 }
149
150 mBiped->setForces(mTorques);
151
152 mFrame++;
153 }
154
customPostStep()155 void customPostStep()
156 {
157 // Use this function to execute custom code after each simulation time
158 // step is performed. This function can be deleted if it does not need
159 // to be used.
160 }
161
162 protected:
163 dart::dynamics::SkeletonPtr mBiped;
164
165 dart::dynamics::BodyNodePtr mLeftHeel;
166 Eigen::VectorXd mTorques;
167 Eigen::VectorXd mDesiredDofs;
168 Eigen::MatrixXd mKp;
169 Eigen::MatrixXd mKd;
170 std::size_t mLeftFoot[2];
171 std::size_t mRightFoot[2];
172 int mFrame;
173 double mTimestep;
174 double mPreOffset;
175 };
176
177 //==============================================================================
178 class CustomEventHandler : public osgGA::GUIEventHandler
179 {
180 public:
CustomEventHandler()181 CustomEventHandler(/*Pass in any necessary arguments*/)
182 {
183 // Set up the customized event handler
184 }
185
handle(const osgGA::GUIEventAdapter & ea,osgGA::GUIActionAdapter &)186 bool handle(
187 const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter&) override
188 {
189 if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN)
190 {
191 if (ea.getKey() == 'q')
192 {
193 std::cout << "Lowercase q pressed" << std::endl;
194 return true;
195 }
196 else if (ea.getKey() == 'Q')
197 {
198 std::cout << "Capital Q pressed" << std::endl;
199 return true;
200 }
201 else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Left)
202 {
203 std::cout << "Left arrow key pressed" << std::endl;
204 return true;
205 }
206 else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Right)
207 {
208 std::cout << "Right arrow key pressed" << std::endl;
209 return true;
210 }
211 }
212 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP)
213 {
214 if (ea.getKey() == 'q')
215 {
216 std::cout << "Lowercase q released" << std::endl;
217 return true;
218 }
219 else if (ea.getKey() == 'Q')
220 {
221 std::cout << "Capital Q released" << std::endl;
222 return true;
223 }
224 else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Left)
225 {
226 std::cout << "Left arrow key released" << std::endl;
227 return true;
228 }
229 else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Right)
230 {
231 std::cout << "Right arrow key released" << std::endl;
232 return true;
233 }
234 }
235
236 // The return value should be 'true' if the input has been fully handled
237 // and should not be visible to any remaining event handlers. It should be
238 // false if the input has not been fully handled and should be viewed by
239 // any remaining event handlers.
240 return false;
241 }
242 };
243
244 //==============================================================================
main()245 int main()
246 {
247 // Create a world and add the rigid body
248 auto world
249 = dart::utils::SkelParser::readWorld("dart://sample/skel/fullbody1.skel");
250 world->setGravity(Eigen::Vector3d(0, -9.81, 0));
251
252 auto biped = world->getSkeleton("fullbody1");
253 biped = world->getSkeleton("fullbody1");
254 biped->getDof("j_pelvis_rot_y")->setPosition(-0.20);
255 biped->getDof("j_thigh_left_z")->setPosition(0.15);
256 biped->getDof("j_shin_left")->setPosition(-0.40);
257 biped->getDof("j_heel_left_1")->setPosition(0.25);
258 biped->getDof("j_thigh_right_z")->setPosition(0.15);
259 biped->getDof("j_shin_right")->setPosition(-0.40);
260 biped->getDof("j_heel_right_1")->setPosition(0.25);
261 biped->getDof("j_abdomen_2")->setPosition(0.00);
262
263 // Create a Viewer and set it up with the WorldNode
264 auto viewer = gui::osg::Viewer();
265 auto shadow = gui::osg::WorldNode::createDefaultShadowTechnique(&viewer);
266
267 // Wrap a WorldNode around it
268 viewer.addWorldNode(new CustomWorldNode(world, biped, shadow));
269 viewer.addEventHandler(new CustomEventHandler());
270
271 viewer.addInstructionText("Press space to start simulation.\n");
272 std::cout << viewer.getInstructions() << std::endl;
273
274 // Set up the window to be 640x480
275 viewer.setUpViewInWindow(0, 0, 640, 480);
276
277 // Adjust the viewpoint of the Viewer
278 viewer.getCameraManipulator()->setHomePosition(
279 ::osg::Vec3(3.0f, 1.5f, 3.0f),
280 ::osg::Vec3(0.0f, 0.0f, 0.0f),
281 ::osg::Vec3(0.0f, 1.0f, 0.0f));
282
283 // We need to re-dirty the CameraManipulator by passing it into the viewer
284 // again, so that the viewer knows to update its HomePosition setting
285 viewer.setCameraManipulator(viewer.getCameraManipulator());
286
287 // Begin running the application loop
288 viewer.run();
289
290 return 0;
291 }
292