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
MyWindow()35 MyWindow::MyWindow() : SimWindow()
36 {
37 mBackWheelVelocity = 0.0;
38 mSteeringWheelAngle = 0.0;
39 mK = 0.01;
40 mD = 0.005;
41 }
42
~MyWindow()43 MyWindow::~MyWindow()
44 {
45 }
46
timeStepping()47 void MyWindow::timeStepping()
48 {
49 dart::dynamics::SkeletonPtr vehicle = mWorld->getSkeleton("car_skeleton");
50 assert(vehicle != 0);
51
52 std::size_t dof = vehicle->getNumDofs();
53
54 Eigen::VectorXd q = vehicle->getPositions();
55 Eigen::VectorXd dq = vehicle->getVelocities();
56 Eigen::VectorXd tau = Eigen::VectorXd::Zero(dof);
57
58 tau[6] = -mK * (q[6] - mSteeringWheelAngle) - mD * dq[6];
59 tau[8] = -mK * (q[8] - mSteeringWheelAngle) - mD * dq[8];
60 tau[7] = -mD * (dq[7] - mBackWheelVelocity);
61 tau[9] = -mD * (dq[9] - mBackWheelVelocity);
62 tau[10] = -mD * (dq[10] - mBackWheelVelocity);
63 tau[11] = -mD * (dq[11] - mBackWheelVelocity);
64
65 vehicle->setForces(tau);
66
67 mWorld->step();
68 }
69
drawWorld() const70 void MyWindow::drawWorld() const
71 {
72 glEnable(GL_LIGHTING);
73 glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
74
75 SimWindow::drawWorld();
76 }
77
keyboard(unsigned char _key,int _x,int _y)78 void MyWindow::keyboard(unsigned char _key, int _x, int _y)
79 {
80
81 using namespace dart::math::suffixes;
82
83 switch (_key)
84 {
85 case ' ': // use space key to play or stop the motion
86 mSimulating = !mSimulating;
87 if (mSimulating)
88 mPlay = false;
89 break;
90 case 'p': // playBack
91 mPlay = !mPlay;
92 if (mPlay)
93 mSimulating = false;
94 break;
95 case '[': // step backward
96 if (!mSimulating)
97 {
98 mPlayFrame--;
99 if (mPlayFrame < 0)
100 mPlayFrame = 0;
101 glutPostRedisplay();
102 }
103 break;
104 case ']': // step forwardward
105 if (!mSimulating)
106 {
107 mPlayFrame++;
108 if (mPlayFrame >= mWorld->getRecording()->getNumFrames())
109 mPlayFrame = 0;
110 glutPostRedisplay();
111 }
112 break;
113 case 'v': // show or hide markers
114 mShowMarkers = !mShowMarkers;
115 break;
116 case 'w': // move forward
117 mBackWheelVelocity = -420.0_deg;
118 break;
119 case 's': // stop
120 mBackWheelVelocity = 0.0_deg;
121 break;
122 case 'x': // move backward
123 mBackWheelVelocity = +420.0_deg;
124 break;
125 case 'a': // rotate steering wheels to left
126 mSteeringWheelAngle += +10_deg;
127 if (mSteeringWheelAngle > 30.0_deg)
128 mSteeringWheelAngle = 30.0_deg;
129 break;
130 case 'd': // rotate steering wheels to right
131 mSteeringWheelAngle += -10_deg;
132 if (mSteeringWheelAngle < -30.0_deg)
133 mSteeringWheelAngle = -30.0_deg;
134 break;
135 default:
136 Win3D::keyboard(_key, _x, _y);
137 }
138 glutPostRedisplay();
139 }
140