NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgTensionController.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 
27 #include "tgTensionController.h"
28 
29 #include "core/tgBasicActuator.h"
30 #include "core/tgSpringCable.h"
31 #include "core/tgCast.h"
32 
33 // The C++ Standard Library
34 #include <cassert>
35 #include <stdexcept>
36 #include <cstddef> // NULL keyword
37 
39 m_sca(controllable),
40 tgBasicController(controllable, setPoint)
41 {
42  assert(controllable != NULL);
43 }
44 
46 {
47  m_sca = NULL;
48 }
49 
51 {
52  if (dt <= 0.0)
53  {
54  throw std::runtime_error ("Timestep must be positive.");
55  }
56 
57  const tgSpringCable* m_springCable = m_sca->getSpringCable();
58 
59  const double stiffness = m_springCable->getCoefK();
60  // @todo: write invariant that checks this;
61  assert(stiffness > 0.0);
62 
63  const double currentTension = m_springCable->getTension();
64  const double delta = m_setPoint - currentTension;
65  double diff = delta / stiffness;
66  const double currentLength = m_springCable->getRestLength();
67 
68  double newLength = m_sca->getRestLength() - diff;
69 
70  // Safety check
71  newLength = newLength < 0.0 ? 0.0 : newLength;
72 
73  m_sca->setControlInput(newLength, dt);
74 }
75 
76 void tgTensionController::control(double dt, double setPoint, double sensorData)
77 {
78  if (dt <= 0.0)
79  {
80  throw std::runtime_error ("Timestep must be positive.");
81  }
82 
83  // Suppress compiler warning for unused variable
84  (void)sensorData;
85 
86  setNewSetPoint(setPoint);
87  control(dt);
88 }
89 
90 void tgTensionController::control(tgBasicActuator& sca, double dt, double setPoint)
91 {
92  if (dt <= 0.0)
93  {
94  throw std::runtime_error ("Timestep must be positive.");
95  }
96 
97  const tgSpringCable* m_springCable = sca.getSpringCable();
98 
99  const double stiffness = m_springCable->getCoefK();
100  // @todo: write invariant that checks this;
101  assert(stiffness > 0.0);
102 
103  const double currentTension = m_springCable->getTension();
104  const double delta = setPoint - currentTension;
105  double diff = delta / stiffness;
106  const double currentLength = m_springCable->getRestLength();
107 
108  double newLength = sca.getRestLength() - diff;
109 
110  // Safety check
111  newLength = newLength < 0.1 ? 0.1 : newLength;
112 
113  sca.setControlInput(newLength, dt);
114 }
Definitions of class tgSpringCable.
Definition of the tgTensionController base class.
Utility class for class casting and filtering collections by type.
virtual void setControlInput(double input)
virtual void control(double dt)
virtual const double getRestLength() const
virtual void setNewSetPoint(double newSetPoint)
Contains the definition of class tgBasicActuator.
virtual const double getCoefK() const
Definition: tgSpringCable.h:95
virtual const double getTension() const =0
tgTensionController(tgBasicActuator *controllable, double setPoint=0.0)
virtual const double getRestLength() const
const tgSpringCable * getSpringCable() const