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 <iostream>
36
37 //==============================================================================
MyWindow(Controller * _controller)38 MyWindow::MyWindow(Controller* _controller)
39 : SimWindow(), mController(_controller), mCircleTask(false)
40 {
41 assert(_controller != nullptr);
42
43 // Set the initial target positon to the initial position of the end effector
44 mTargetPosition = mController->getEndEffector()->getTransform().translation();
45 }
46
47 //==============================================================================
~MyWindow()48 MyWindow::~MyWindow()
49 {
50 }
51
52 //==============================================================================
timeStepping()53 void MyWindow::timeStepping()
54 {
55 if (mCircleTask)
56 {
57 static double time = 0.0;
58 const double dt = 0.0005;
59 const double radius = 0.6;
60 Eigen::Vector3d center = Eigen::Vector3d(0.0, 0.1, 0.0);
61
62 mTargetPosition = center;
63 mTargetPosition[0] = radius * std::sin(time);
64 mTargetPosition[1] = 0.25 * radius * std::sin(time);
65 mTargetPosition[2] = radius * std::cos(time);
66
67 time += dt;
68 }
69
70 // Update the controller and apply control force to the robot
71 mController->update(mTargetPosition);
72
73 // Step forward the simulation
74 mWorld->step();
75 }
76
77 //==============================================================================
drawWorld() const78 void MyWindow::drawWorld() const
79 {
80 // Draw the target position
81 if (mRI)
82 {
83 mRI->setPenColor(Eigen::Vector3d(0.8, 0.2, 0.2));
84 mRI->pushMatrix();
85 mRI->translate(mTargetPosition);
86 mRI->drawEllipsoid(Eigen::Vector3d(0.05, 0.05, 0.05));
87 mRI->popMatrix();
88 }
89
90 // Draw world
91 SimWindow::drawWorld();
92 }
93
94 //==============================================================================
keyboard(unsigned char _key,int _x,int _y)95 void MyWindow::keyboard(unsigned char _key, int _x, int _y)
96 {
97 double incremental = 0.01;
98
99 switch (_key)
100 {
101 case 'c': // print debug information
102 if (mCircleTask)
103 {
104 std::cout << "Circle task [off]." << std::endl;
105 mCircleTask = false;
106 }
107 else
108 {
109 std::cout << "Circle task [on]." << std::endl;
110 mCircleTask = true;
111 }
112 break;
113 case 'q':
114 mTargetPosition[0] -= incremental;
115 break;
116 case 'w':
117 mTargetPosition[0] += incremental;
118 break;
119 case 'a':
120 mTargetPosition[1] -= incremental;
121 break;
122 case 's':
123 mTargetPosition[1] += incremental;
124 break;
125 case 'z':
126 mTargetPosition[2] -= incremental;
127 break;
128 case 'x':
129 mTargetPosition[2] += incremental;
130 break;
131 default:
132 // Default keyboard control
133 SimWindow::keyboard(_key, _x, _y);
134 break;
135 }
136
137 // Keyboard control for Controller
138 mController->keyboard(_key, _x, _y);
139
140 glutPostRedisplay();
141 }
142