NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
T12SuperBallPayloadController.cpp
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 
27 // This module
28 #include "T12SuperBallPayloadController.h"
29 // This application
30 #include "T12SuperBallPayload.h"
31 // This library
32 #include "core/tgBasicActuator.h"
33 // The C++ Standard Library
34 #include <cassert>
35 #include <stdexcept>
36 #include <vector>
37 
38 using namespace std;
39 
40 //Constructor using the model subject and a single pref length for all muscles.
42 {
43  this->m_initialLengths=initialLength;
44  this->m_totalTime=0.0;
45 }
46 
47 //Fetch all the muscles and set their preferred length
49 {
50  const std::vector<tgBasicActuator*> muscles = subject.getAllMuscles();
51  for (size_t i = 0; i < muscles.size(); ++i)
52  {
53  tgBasicActuator * const pMuscle = muscles[i];
54  assert(pMuscle != NULL);
55  pMuscle->setControlInput(this->m_initialLengths,0.0001);
56  }
57 }
58 
60 {
61  if (dt <= 0.0)
62  {
63  throw std::invalid_argument("dt is not positive");
64  }
65  m_totalTime+=dt;
66 
67  //Move motors for all the muscles
68  const std::vector<tgBasicActuator*> muscles = subject.getAllMuscles();
69  for (size_t i = 0; i < muscles.size(); ++i)
70  {
71  tgBasicActuator * const pMuscle = muscles[i];
72  assert(pMuscle != NULL);
73  pMuscle->moveMotors(dt);
74  }
75 
76  //vector<double> state=getState();
77  vector< vector<double> > actions;
78 
79  //get the actions (between 0 and 1) from evolution (todo)
80  //actions=evolutionAdapter.step(dt,state);
81 
82  //instead, generate it here for now!
83  for(int i=0;i<24;i++)
84  {
85  vector<double> tmp;
86  for(int j=0;j<2;j++)
87  {
88  tmp.push_back(0.5);
89  }
90  actions.push_back(tmp);
91  }
92 
93  //transform them to the size of the structure
94  actions = transformActions(actions);
95 
96  //apply these actions to the appropriate muscles according to the sensor values
97 // applyActions(subject,actions);
98 
99 }
100 
101 //Scale actions according to Min and Max length of muscles.
102 vector< vector <double> > SuperBallPrefLengthController::transformActions(vector< vector <double> > actions)
103 {
104  double min=6;
105  double max=11;
106  double range=max-min;
107  double scaledAct;
108  for(int i=0;i<actions.size();i++)
109  {
110  for(int j=0;j<actions[i].size();j++)
111  {
112  scaledAct=actions[i][j]*(range)+min;
113  actions[i][j]=scaledAct;
114  }
115  }
116  return actions;
117 }
118 
119 //Pick particular muscles (according to the structure's state) and apply the given actions one by one
120 void SuperBallPrefLengthController::applyActions(T12SuperBallPayload& subject, vector< vector <double> > act)
121 {
122  //Get All the muscles of the subject
123  const std::vector<tgBasicActuator*> muscles = subject.getAllMuscles();
124  //Check if the number of the actions match the number of the muscles
125  if(act.size() != muscles.size())
126  {
127  cout<<"Warning: # of muscles: "<< muscles.size() << " != # of actions: "<< act.size()<<endl;
128  return;
129  }
130  //Apply actions (currently in a random order)
131  for (size_t i = 0; i < muscles.size(); ++i)
132  {
133  tgBasicActuator * const pMuscle = muscles[i];
134  assert(pMuscle != NULL);
135  //cout<<"i: "<<i<<" length: "<<act[i][0]<<endl;
136  pMuscle->setControlInput(act[i][0]);
137  }
138 }
Contains the definition of class T12SuperBallPayload. $Id$.
virtual void moveMotors(double dt)
virtual void setControlInput(double input)
SuperBallPrefLengthController(const double prefLength=5)
Contains the definition of class tgBasicActuator.
virtual void onSetup(SuperBallModel &subject)
const std::vector< tgBasicActuator * > & getAllMuscles() const
virtual void onStep(SuperBallModel &subject, double dt)