NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
JSONAchillesHierarchyControl.h
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 
19 #ifndef JSON_ACHILLES_HIERARCHY_CONTROL_H
20 #define JSON_ACHILLES_HIERARCHY_CONTROL_H
21 
31 #include "dev/dhustigschultz/BP_SC_NoLegs_Stats/JSONQuadCPGControl.h"
32 
33 #include <json/value.h>
34 
35 // Forward Declarations
36 class neuralNetwork;
38 
39 
49 {
50 public:
51 
53  {
54  public:
58  Config( int ss,
59  int tm,
60  int om,
61  int param,
62  int segnum = 6,
63  double ct = 0.1,
64  double la = 0,
65  double ha = 30,
66  double lp = -1 * M_PI,
67  double hp = M_PI,
68  double kt = 0.0,
69  double kp = 1000.0,
70  double kv = 100.0,
71  bool def = true,
72  double cl = 10.0,
73  double lf = 0.0,
74  double hf = 30.0,
75  double ffMin = 0.0,
76  double ffMax = 0.0,
77  double afMin = 0.0,
78  double afMax = 0.0,
79  double pfMin = 0.0,
80  double pfMax = 0.0,
81  double maxH = 60.0, //May need to tune this value more
82  double minH = 1.0, //Perhaps same
83  int ohm = 10,
84  int thm = 10,
85  int olm = 10,
86  int tlm = 10,
87  int oam = 2,
88  int tam = 2,
89  int ohighm = 5,
90  int thighm = 5,
91  double hf2 = 20.0,
92  double ffMax2 = 0.0
93  );
94 
95  const double freqFeedbackMin;
96  const double freqFeedbackMax;
97  const double ampFeedbackMin;
98  const double ampFeedbackMax;
99  const double phaseFeedbackMin;
100  const double phaseFeedbackMax;
101 
102  //So that these limits don't have to be hardcoded:
103  const double maxHeight;
104  const double minHeight;
105 
106  // Values to be filled in by JSON file during onSetup
107  int numStates;
108  int numActions;
109 
110  // New values of numMuscles, for non-spine segments:
111  int ourHipMuscles;
112  int theirHipMuscles;
113  int ourLegMuscles;
114  int theirLegMuscles;
115  int ourAchillesMuscles;
116  int theirAchillesMuscles;
117  int ourHighMuscles;
118  int theirHighMuscles;
119 
120  // These are for the higher level CPGs
121  const double highFreq2;
122  const double freqFeedbackMax2;
123  };
124 
126  std::string args,
127  std::string resourcePath = "");
128 
129  virtual ~JSONAchillesHierarchyControl();
130 
131  virtual void onSetup(BaseQuadModelLearning& subject);
132 
133  virtual void onStep(BaseQuadModelLearning& subject, double dt);
134 
135  virtual void onTeardown(BaseQuadModelLearning& subject);
136 
137 protected:
138 
139  virtual void setupCPGs(BaseQuadModelLearning& subject, array_2D spineNodeActions, array_2D legNodeActions, array_4D spineEdgeActions, array_4D hipEdgeActions, array_4D legEdgeActions, array_4D achillesEdgeActions);
140 
141  virtual void setupHighCPGs(BaseQuadModelLearning& subject, array_2D highNodeActions, array_4D highEdgeActions);
142 
143  //If need to couple the other way, then may need to pass more arrays as parameters
144  virtual void setupHighLowCouplings(BaseQuadModelLearning& subject, Json::Value highLowEdgeActions);
145 
146  virtual array_2D scaleNodeActions (Json::Value actions, double highFreq, double freqFeedbackMax);
147 
148  virtual array_4D scaleEdgeActions (Json::Value actions, int segmentSpan, int theirMuscles, int ourMuscles);
149 
150  std::vector<double> getFeedback(BaseQuadModelLearning& subject);
151 
152  std::vector<double> getCableState(const tgSpringCableActuator& cable);
153 
154  std::vector<double> transformFeedbackActions(std::vector< std::vector<double> >& actions);
155 
157 
158  // @todo: doing separate vectors of controllers, until I can find a way to make them into a vector of vectors, without segfaulting.
159  std::vector<tgCPGActuatorControl*> m_spineControllers;
160 
161  std::vector<tgCPGActuatorControl*> m_leftShoulderControllers;
162  std::vector<tgCPGActuatorControl*> m_rightShoulderControllers;
163 
164  std::vector<tgCPGActuatorControl*> m_leftHipControllers;
165  std::vector<tgCPGActuatorControl*> m_rightHipControllers;
166 
167  std::vector<tgCPGActuatorControl*> m_leftForelegControllers;
168  std::vector<tgCPGActuatorControl*> m_rightForelegControllers;
169 
170  std::vector<tgCPGActuatorControl*> m_leftHindlegControllers;
171  std::vector<tgCPGActuatorControl*> m_rightHindlegControllers;
172 
173  std::vector<tgCPGActuatorControl*> m_leftFrontAchillesControllers;
174  std::vector<tgCPGActuatorControl*> m_rightFrontAchillesControllers;
175 
176  std::vector<tgCPGActuatorControl*> m_leftRearAchillesControllers;
177  std::vector<tgCPGActuatorControl*> m_rightRearAchillesControllers;
178 
179  std::vector<tgCPGActuatorControl*> m_highControllers;
180 
181  // @todo generalize this if we need more than one
182  neuralNetwork* nn;
183 
184  std::vector< std::vector<double> > m_quadCOM;
185 
186 };
187 
188 #endif // JSON_ACHILLES_HIERARCHY_CONTROL_H
virtual void onTeardown(BaseQuadModelLearning &subject)
JSONAchillesHierarchyControl(JSONAchillesHierarchyControl::Config config, std::string args, std::string resourcePath="")
virtual void onStep(BaseQuadModelLearning &subject, double dt)
virtual array_4D scaleEdgeActions(Json::Value actions, int segmentSpan, int theirMuscles, int ourMuscles)
Config(int ss, int tm, int om, int param, int segnum=6, double ct=0.1, double la=0, double ha=30, double lp=-1 *M_PI, double hp=M_PI, double kt=0.0, double kp=1000.0, double kv=100.0, bool def=true, double cl=10.0, double lf=0.0, double hf=30.0, double ffMin=0.0, double ffMax=0.0, double afMin=0.0, double afMax=0.0, double pfMin=0.0, double pfMax=0.0, double maxH=60.0, double minH=1.0, int ohm=10, int thm=10, int olm=10, int tlm=10, int oam=2, int tam=2, int ohighm=5, int thighm=5, double hf2=20.0, double ffMax2=0.0)
virtual void onSetup(BaseQuadModelLearning &subject)