NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
BigPuppyController.cpp
Go to the documentation of this file.
1 /*
2  * Copyright © 2012, United States Government, as represented by the
3  * Administrator of the National Aeronautics and Space Administration.
4  * All rights reserved.
5  *
6  * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
7  * under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0.
11  *
12  * Unless required by applicable law or agreed to in writing,
13  * software distributed under the License is distributed on an
14  * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
15  * either express or implied. See the License for the specific language
16  * governing permissions and limitations under the License.
17 */
18 
28 //This module
29 #include "BigPuppyController.h"
30 //This application
31 #include "BigPuppy.h"
32 
33 //This library
35 #include "core/tgBasicActuator.h"
37 
38 //The C++ standard library
39 #include <string>
40 #include <stdexcept>
41 #include <cassert>
42 #include <vector>
43 #include <cmath>
44 
45 
46 # define M_PI 3.14159265358979323846
47 
48 BigPuppyController::BigPuppyController(double timestep) :
49  m_totalTime(0.0),
50  dt(timestep) {}
51 
53  this->m_totalTime=0.0;
54 }
55 
56 void BigPuppyController::onStep(BigPuppy& subject, double dt){
57 
58  // Update controller's internal time
59  if (dt <= 0.0) { throw std::invalid_argument("dt is not positive"); }
60  m_totalTime+=dt;
61 
62  setBicepTargetLength(subject, dt);
63  setFrontTricepTargetLength(subject, dt);
64  setRearTricepTargetLength(subject, dt);
65  setLegToAbdomenTargetLength(subject, dt);
66  setRightShoulderTargetLength(subject, dt);
67  moveAllMotors(subject, dt);
68 }
69 
71 
72 }
73 
74 void BigPuppyController::moveAllMotors(BigPuppy& subject, double dt){
75 
76  const std::vector<tgSpringCableActuator*> muscles = subject.getAllMuscles();
77  for (size_t i = 0; i < muscles.size(); ++i) {
78  tgBasicActuator * const pMuscle = tgCast::cast<tgSpringCableActuator, tgBasicActuator>(muscles[i]);
79  assert(pMuscle != NULL);
80  pMuscle->moveMotors(dt);
81  }
82 
83 }
84 
85 void BigPuppyController::setBicepTargetLength(BigPuppy& subject, double dt){
86 
87  double newLengthMid = 0;
88  double newLengthOuter = 0;
89  const double bicepLength = 15.0;
90 
91  const double amplitude1 = bicepLength/2;
92  const double amplitude2 = bicepLength/2;
93  const double angular_freq = 2;
94  const double phaseMid = 3*M_PI/2;
95  const double phaseOuter = phaseMid - M_PI;
96  const double dcOffset = bicepLength/2;
97 
98  const std::vector<tgBasicActuator*> bicepMid = subject.find<tgBasicActuator>("right mid bicep");
99  const std::vector<tgBasicActuator*> bicepOuter = subject.find<tgBasicActuator>("right outer bicep");
100  const std::vector<tgBasicActuator*> bicepInner = subject.find<tgBasicActuator>("right inner bicep");
101 
102  assert(bicepMid[0] != NULL);
103  assert(bicepOuter[0] != NULL);
104  assert(bicepInner[0] != NULL);
105 
106  newLengthMid = amplitude1 * sin(angular_freq * m_totalTime + phaseMid) + dcOffset;
107  newLengthOuter = amplitude2 * sin(angular_freq * m_totalTime + phaseOuter) + 2*dcOffset;
108 
109  bicepMid[0]->setControlInput(newLengthMid);
110  bicepOuter[0]->setControlInput(newLengthOuter);
111  bicepInner[0]->setControlInput(newLengthOuter);
112 
113 }
114 
115 void BigPuppyController::setFrontTricepTargetLength(BigPuppy& subject, double dt){
116 
117  double newLengthInner = 0;
118  double newLengthOuter = 0;
119  const double frontTricepLength = sqrt(245.0);
120 
121  const double amplitude = frontTricepLength/4;
122  const double angular_freq = 2;
123  const double phase1 = M_PI/2;
124  const double phase2 = 3*M_PI/2;
125  const double dcOffset = frontTricepLength;
126 
127 
128  const std::vector<tgBasicActuator*> frontOuterTricep = subject.find<tgBasicActuator>("outer right front tricep");
129  const std::vector<tgBasicActuator*> frontInnerTricep = subject.find<tgBasicActuator>("inner right front tricep");
130 
131  assert(frontOuterTricep[0] != NULL);
132  assert(frontInnerTricep[0] != NULL);
133 
134  newLengthInner = dcOffset - amplitude * sin(angular_freq * m_totalTime + phase1);
135  newLengthOuter = dcOffset - amplitude * sin(angular_freq * m_totalTime + phase2);
136 
137  frontOuterTricep[0]->setControlInput(newLengthOuter);
138  frontInnerTricep[0]->setControlInput(newLengthInner);
139 
140 }
141 
142 void BigPuppyController::setRearTricepTargetLength(BigPuppy& subject, double dt){
143 
144  double newLengthInner = 0;
145  double newLengthOuter = 0;
146  const double rearTricepLength = sqrt(165);
147 
148  const double amplitude = rearTricepLength/4;
149  const double angular_freq = 2;
150  const double phase1 = 3*M_PI/2;
151  const double phase2 = M_PI/2;
152  const double dcOffset = rearTricepLength;
153 
154  const std::vector<tgBasicActuator*> rearOuterTricep = subject.find<tgBasicActuator>("outer right tricep");
155  const std::vector<tgBasicActuator*> rearInnerTricep = subject.find<tgBasicActuator>("inner right tricep");
156 
157  assert(rearOuterTricep[0] != NULL);
158  assert(rearInnerTricep[0] != NULL);
159 
160  newLengthInner = dcOffset - amplitude * sin(angular_freq * m_totalTime + phase1);
161  newLengthOuter = dcOffset - amplitude * sin(angular_freq * m_totalTime + phase2);
162 
163  rearOuterTricep[0]->setControlInput(newLengthOuter);
164  rearInnerTricep[0]->setControlInput(newLengthInner);
165 
166 }
167 
168 void BigPuppyController::setLegToAbdomenTargetLength(BigPuppy& subject, double dt){
169 
170  double newLength = 0;
171  const double legToAbdomenLength = sqrt(66);
172 
173  const double amplitude = legToAbdomenLength/1; //See if this needs to change much first before throwing in an amplitude.
174  const double angular_freq = 2;
175  const double phase = 3*M_PI/2;
176  const double dcOffset = legToAbdomenLength;
177 
178  const std::vector<tgBasicActuator*> legToAbdomen = subject.find<tgBasicActuator>("right front abdomen");
179 
180  assert(legToAbdomen[0] != NULL);
181 
182  newLength = amplitude * sin(angular_freq * m_totalTime + phase) + dcOffset;
183 
184  legToAbdomen[0]->setControlInput(newLength);
185 
186 }
187 
188 
189 void BigPuppyController::setRightShoulderTargetLength(BigPuppy& subject, double dt){
190 
191  double newLengthFront = 0;
192  double newLengthRear = 0;
193  const double rightShoulderLength = sqrt(101);
194 
195  const double amplitude = rightShoulderLength/2; //See if this needs to change much first before throwing in an amplitude.
196  const double angular_freq = 2;
197  const double phaseRear = 3*M_PI/2;
198  const double phaseFront = M_PI/2;
199  const double dcOffset = rightShoulderLength;
200 
201  const std::vector<tgBasicActuator*> rightShoulderFront = subject.find<tgBasicActuator>("right shoulder front mid");
202  const std::vector<tgBasicActuator*> rightShoulderRear = subject.find<tgBasicActuator>("right shoulder rear mid");
203 
204  assert(rightShoulderFront[0] != NULL);
205  assert(rightShoulderRear[0] != NULL);
206 
207  newLengthFront = dcOffset - amplitude * sin(angular_freq * m_totalTime + phaseFront);
208  newLengthRear = dcOffset - amplitude * sin(angular_freq * m_totalTime + phaseRear);
209 
210  rightShoulderFront[0]->setControlInput(newLengthFront);
211  rightShoulderRear[0]->setControlInput(newLengthRear);
212 
213 }
virtual void onStep(BigPuppy &subject, double dt)
Implementing a hand-tuned controller for a quadruped based roughly on the Flemons BigPuppy model...
virtual void moveMotors(double dt)
virtual void onSetup(BigPuppy &subject)
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
virtual void onTeardown(BigPuppy &subject)
Contains the definition of class tgBasicActuator.
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition: tgModel.h:128
Contains the definition of class tgKinematicActuator.
const std::vector< tgSpringCableActuator * > & getAllMuscles() const
Definition: BigPuppy.cpp:658