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 "MyWindow.hpp"
34
35 #include <dart/gui/GLFuncs.hpp>
36
MyWindow()37 MyWindow::MyWindow() : SimWindow()
38 {
39 mForce = Eigen::Vector3d::Zero();
40 mController = nullptr;
41 mImpulseDuration = 0;
42 }
43
~MyWindow()44 MyWindow::~MyWindow()
45 {
46 }
47
timeStepping()48 void MyWindow::timeStepping()
49 {
50 mWorld->getSkeleton(1)->getBodyNode("h_spine")->addExtForce(mForce);
51
52 mController->computeTorques();
53 mController->getSkel()->setForces(mController->getTorques());
54
55 mWorld->step();
56
57 // for perturbation test
58 mImpulseDuration--;
59 if (mImpulseDuration <= 0)
60 {
61 mImpulseDuration = 0;
62 mForce.setZero();
63 }
64 }
65
drawWorld() const66 void MyWindow::drawWorld() const
67 {
68
69 SimWindow::drawWorld();
70
71 // draw arrow
72 if (mImpulseDuration > 0)
73 {
74 Eigen::Vector3d poa
75 = mWorld->getSkeleton(1)->getBodyNode("h_spine")->getTransform()
76 * Eigen::Vector3d(0.0, 0.0, 0.0);
77 Eigen::Vector3d start = poa - mForce / 10.0;
78 double len = mForce.norm() / 10.0;
79 dart::gui::drawArrow3D(start, mForce, len, 0.05, 0.1);
80 }
81 }
82
keyboard(unsigned char _key,int _x,int _y)83 void MyWindow::keyboard(unsigned char _key, int _x, int _y)
84 {
85 switch (_key)
86 {
87 case ' ': // use space key to play or stop the motion
88 mSimulating = !mSimulating;
89 if (mSimulating)
90 mPlay = false;
91 break;
92 case 'p': // playBack
93 mPlay = !mPlay;
94 if (mPlay)
95 mSimulating = false;
96 break;
97 case '[': // step backward
98 if (!mSimulating)
99 {
100 mPlayFrame--;
101 if (mPlayFrame < 0)
102 mPlayFrame = 0;
103 glutPostRedisplay();
104 }
105 break;
106 case ']': // step forwardward
107 if (!mSimulating)
108 {
109 mPlayFrame++;
110 if (mPlayFrame >= mWorld->getRecording()->getNumFrames())
111 mPlayFrame = 0;
112 glutPostRedisplay();
113 }
114 break;
115 case 'v': // show or hide markers
116 mShowMarkers = !mShowMarkers;
117 break;
118 case '1':
119 mForce[0] = 50;
120 mImpulseDuration = 100.0;
121 std::cout << "push forward" << std::endl;
122 break;
123 case '2':
124 mForce[0] = -50;
125 mImpulseDuration = 100.0;
126 std::cout << "push backward" << std::endl;
127 break;
128 case '3':
129 mForce[2] = 50;
130 mImpulseDuration = 100.0;
131 std::cout << "push right" << std::endl;
132 break;
133 case '4':
134 mForce[2] = -50;
135 mImpulseDuration = 100.0;
136 std::cout << "push left" << std::endl;
137 break;
138 case 'g':
139 plotCOMX();
140 break;
141 default:
142 Win3D::keyboard(_key, _x, _y);
143 }
144 glutPostRedisplay();
145 }
146
setController(Controller * _controller)147 void MyWindow::setController(Controller* _controller)
148 {
149 mController = _controller;
150 }
151
plotCOMX()152 void MyWindow::plotCOMX()
153 {
154 int nFrame = mWorld->getRecording()->getNumFrames();
155 Eigen::VectorXd data(nFrame);
156 for (int i = 0; i < nFrame; i++)
157 {
158 Eigen::VectorXd pose = mWorld->getRecording()->getConfig(i, 1);
159 mWorld->getSkeleton(1)->setPositions(pose);
160 data[i] = mWorld->getSkeleton(1)->getCOM()[0];
161 }
162 if (nFrame != 0)
163 {
164 Eigen::VectorXd pose = mWorld->getRecording()->getConfig(mPlayFrame, 1);
165 mWorld->getSkeleton(1)->setPositions(pose);
166 }
167
168 plot(data);
169 }
170