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
37 //==============================================================================
MyWindow(Controller * _controller)38 MyWindow::MyWindow(Controller* _controller)
39 : SimWindow(), mController(_controller)
40 {
41 mForce = Eigen::Vector3d::Zero();
42 mImpulseDuration = 0.0;
43 }
44
45 //==============================================================================
~MyWindow()46 MyWindow::~MyWindow()
47 {
48 delete mController;
49 }
50
51 //==============================================================================
timeStepping()52 void MyWindow::timeStepping()
53 {
54 // External force
55 mWorld->getSkeleton("drc_skeleton")
56 ->getBodyNode("pelvis")
57 ->addExtForce(mForce);
58
59 // Internal force
60 mController->update(mWorld->getTime());
61
62 // simulate one step
63 mWorld->step();
64
65 // for perturbation test
66 mImpulseDuration--;
67 if (mImpulseDuration <= 0)
68 {
69 mImpulseDuration = 0;
70 mForce.setZero();
71 }
72 }
73
74 //==============================================================================
drawSkels()75 void MyWindow::drawSkels()
76 {
77 // glEnable(GL_LIGHTING);
78 // glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
79
80 for (unsigned int i = 0; i < mWorld->getNumSkeletons(); i++)
81 drawSkeleton(mWorld->getSkeleton(i).get());
82
83 // draw arrow
84 if (mImpulseDuration > 0)
85 {
86 Eigen::Vector3d poa = mWorld->getSkeleton("drc_skeleton")
87 ->getBodyNode("pelvis")
88 ->getTransform()
89 * Eigen::Vector3d(0.0, 0.0, 0.0);
90 Eigen::Vector3d start = poa - mForce / 500.0;
91 double len = mForce.norm() / 500.0;
92 dart::gui::drawArrow3D(start, mForce, len, 0.05, 0.1);
93 }
94 }
95
96 //==============================================================================
keyboard(unsigned char _key,int _x,int _y)97 void MyWindow::keyboard(unsigned char _key, int _x, int _y)
98 {
99 switch (_key)
100 {
101 case ' ': // use space key to play or stop the motion
102 mSimulating = !mSimulating;
103 if (mSimulating)
104 mPlay = false;
105 break;
106 case 'p': // playBack
107 mPlay = !mPlay;
108 if (mPlay)
109 mSimulating = false;
110 break;
111 case '[': // step backward
112 if (!mSimulating)
113 {
114 mPlayFrame--;
115 if (mPlayFrame < 0)
116 mPlayFrame = 0;
117 glutPostRedisplay();
118 }
119 break;
120 case ']': // step forwardward
121 if (!mSimulating)
122 {
123 mPlayFrame++;
124 if (mPlayFrame >= mWorld->getRecording()->getNumFrames())
125 mPlayFrame = 0;
126 glutPostRedisplay();
127 }
128 break;
129 case 'i': // print debug information
130 mController->printDebugInfo();
131 break;
132 case 'v': // show or hide markers
133 mShowMarkers = !mShowMarkers;
134 break;
135 case 'a': // upper right force
136 mForce[0] = 500;
137 mImpulseDuration = 100;
138 std::cout << "push forward" << std::endl;
139 break;
140 case 's': // upper right force
141 mForce[0] = -500;
142 mImpulseDuration = 100;
143 std::cout << "push backward" << std::endl;
144 break;
145 case 'd': // upper right force
146 mForce[2] = 500;
147 mImpulseDuration = 100;
148 std::cout << "push right" << std::endl;
149 break;
150 case 'f': // upper right force
151 mForce[2] = -500;
152 mImpulseDuration = 100;
153 std::cout << "push left" << std::endl;
154 break;
155 default:
156 Win3D::keyboard(_key, _x, _y);
157 }
158
159 // Keyboard control for Controller
160 mController->keyboard(_key, _x, _y, mWorld->getTime());
161
162 glutPostRedisplay();
163 }
164