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