NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgCompoundRigidSensor.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 // This class:
28 #include "tgCompoundRigidSensor.h"
29 
30 // Includes from NTRT:
31 #include "core/tgSenseable.h"
32 #include "core/tgCast.h"
33 #include "core/tgTags.h"
34 #include "core/tgBaseRigid.h"
35 
36 // Includes from the c++ standard library:
37 //#include <iostream>
38 #include <sstream>
39 #include <stdexcept>
40 #include <cassert>
41 #include <string> // for std::to_string(float)
42 #include <math.h> // for the constant PI (3.14...)
43 
44 // Includes from Bullet Physics:
45 #include "btBulletDynamicsCommon.h" // includes all the Bullet stuff we need.
46 
47 //#include "LinearMath/btVector3.h"
48 //#include "LinearMath/btQuaternion.h" //already included in header.
49 //#include "BulletDynamics/Dynamics/btRigidBody.h" // accessing the btRigids inside m_rigids.
50 
61 {
62  throw std::invalid_argument("Called the constructor for tgCompoundRigidSensor with only a pointer, NOT a tag. That's not allowed, a specific compound tag must be passed in also.");
63 }
64 
70  tgSensor(pModel),
71  m_tag(tag)
72 {
73  // Note that this pointer may be 0 (equivalent to NULL) if the cast in
74  // the calling function from tgSenseable to tgModel fails.
75  if (pModel == NULL) {
76  throw std::invalid_argument("Pointer to pModel is NULL inside tgCompoundRigidSensor.");
77  }
78  // Then, pick out the rigid bodies in the model that have the tag.
79  // tgModel has a nice method for doing this.
80  m_rigids = pModel->find<tgBaseRigid>(m_tag);
81  // Confirm that there is at least one rigid.
82  if( m_rigids.empty() ) {
83  throw std::runtime_error("tgCompoundRigidSensor found no rigid bodies with its tag - something is wrong (inside constructor.)");
84  }
85  // TO-DO: better validation.
86 
87  // Next, get the initial orientation of the rigid body.
88  // This will be needed for comparison later.
89  // TO-DO: make sure this is called AFTER the rigid body is "moved into place"
90  // ...is that what happens in NTRT/Bullet??
91 
92  // (1) get the first Bullet rigid body from the list
93  btRigidBody* firstRigid = m_rigids[0]->getPRigidBody();
94  // (2) store its current orientation
95  origOrientQuat = firstRigid->getOrientation();
96  // (3) have Bullet calculate the inverse quaternion.
97  // That's what will be used below.
98  origOrientQuatInv = origOrientQuat.inverse();
99 }
100 
107 {
108  // However, we should clear the pointers to the rigid bodies.
109  // Again, not delete the objects, just clear the pointers.
110  // Note that tgDataManager calls delete on this sensor during reset
111  // e.g., (teardown/setup.)
112 
113  // TO-DO: implement this.
114 }
115 
116 // Three data collection methods. They should all
117 // return zeroes if m_rigids is empty.
118 // NEED TO DO ASAP: this is INCORRECT, we need to average / integrate
119 // over all the volume of the rigids, not just avg their individual COMs.
120 btVector3 tgCompoundRigidSensor::getCenterOfMass()
121 {
122  // This method takes the average of all the centers of mass.
123  // It should be sufficient to just add then divide each component
124  // of the 3D vector.
125  // The resulting vector:
126  btVector3* pCom = new btVector3(0.0, 0.0, 0.0);
127  // Iterate and add all the centers of mass of the components.
128  for( size_t i=0; i < m_rigids.size(); i++){
129  *pCom += m_rigids[i]->centerOfMass();
130  }
131  // Average the components:
132  *pCom /= m_rigids.size();
133 
134  return *pCom;
135 }
136 
137 btVector3 tgCompoundRigidSensor::getOrientation()
138 {
139  // For now, it will be easier to poke around at the underlying Bullet Physics
140  // objects.
141  // TO-DO: encapsulate this functionality inside tgBaseRigid.
142  // To get the "difference" between wherever the first rigid body was,
143  // and where it is now, multiply Q_curr * inv(Q_0).
144  // First, get the orientation of the underlying Bullet rigid body:
145  btQuaternion currentOrientQuat = m_rigids[0]->getPRigidBody()->getOrientation();
146  // The "difference" is then
147  btQuaternion diffOrientQuat = currentOrientQuat * origOrientQuatInv;
148  // Convert to roll/pitch/yaw just like inside tgBaseRigid::orientation().
149  btMatrix3x3 rotMat = btMatrix3x3( diffOrientQuat );
150  btScalar yaw = 0.0;
151  btScalar pitch = 0.0;
152  btScalar roll = 0.0;
153  rotMat.getEulerYPR(yaw, pitch, roll);
154  // Convert from radians to degrees, since that's what most people
155  // will care about when parsing this data.
156  yaw = 180/M_PI * yaw;
157  pitch = 180/M_PI * pitch;
158  roll = 180/M_PI * roll;
159 
160  // The final result:
161  btVector3 orient = btVector3(yaw, pitch, roll);
162  return orient;
163 
164  // The resulting vector:
165  //btVector3* orient = new btVector3(0.0, 0.0, 0.0);
166 
167  //return *orient;
168 }
169 
170 double tgCompoundRigidSensor::getMass()
171 {
172  // Add the mass of all the rigid bodies.
173  double mass = 0;
174  for( size_t i=0; i < m_rigids.size(); i++){
175  mass += m_rigids[i]->mass();
176  }
177  return mass;
178 }
179 
188  // Note that this class has access to the parent's pointer, m_pSens.
189  // Let's cast that to a pointer to a tgModel right now.
190  // Here, "m_pModel" stands for "my pointer to a tgModel."
191  tgModel* m_pModel = tgCast::cast<tgSenseable, tgModel>(m_pSens);
192  // Check: if the cast failed, this will return 0.
193  // In that case, this tgCompoundRigidSensor does not point to a tgModel!!!
194  assert( m_pModel != 0);
195 
196  // The list to which we'll append all the sensor headings:
197  std::vector<std::string> headings;
198 
199  // Copied from tgSensor.h:
209  // The string 'prefix' will be added to each heading.
210  std::string prefix = "compound(";
211 
212  // Create the headings, similar to tgRod for example.
213  headings.push_back( prefix + m_tag + ").X" );
214  headings.push_back( prefix + m_tag + ").Y" );
215  headings.push_back( prefix + m_tag + ").Z" );
216  headings.push_back( prefix + m_tag + ").YawDegrees" );
217  headings.push_back( prefix + m_tag + ").PitchDegrees" );
218  headings.push_back( prefix + m_tag + ").RollDegrees" );
219  headings.push_back( prefix + m_tag + ").mass" );
220 
221  // Return the resulting vector.
222  return headings;
223 }
224 
228 std::vector<std::string> tgCompoundRigidSensor::getSensorData() {
229  // Note that this method uses m_rigids directly, no need to deal
230  // with the parent class' pointer to m_pSens.
231 
232  // The floats (btScalars?) need to be converted to strings
233  // via a stringstream.
234  std::stringstream ss;
235 
236  // The list of sensor data that will be returned:
237  std::vector<std::string> sensordata;
238 
239  // Get the position and orientation of this compound body.
240  // Call the helper functions
241  btVector3 com = getCenterOfMass();
242  btVector3 orient = getOrientation();
243 
244  // com[0]
245  ss << com[0];
246  sensordata.push_back( ss.str() );
247  // Reset the stream.
248  ss.str("");
249 
250  // com[1]
251  ss << com[1];
252  sensordata.push_back( ss.str() );
253  ss.str("");
254  // com[2]
255  ss << com[2];
256  sensordata.push_back( ss.str() );
257  ss.str("");
258 
259  // yaw
260  ss << orient[0];
261  sensordata.push_back( ss.str() );
262  ss.str("");
263 
264  // pitch
265  ss << orient[1];
266  sensordata.push_back( ss.str() );
267  ss.str("");
268 
269  // roll
270  ss << orient[2];
271  sensordata.push_back( ss.str() );
272  ss.str("");
273 
274  //sensordata.push_back( "" );
275  //sensordata.push_back( "" );
276  //sensordata.push_back( "" );
277 
278  // mass
279  ss << getMass();
280  sensordata.push_back( ss.str() );
281  ss.str("");
282 
283  return sensordata;
284 }
285 
286 //end.
Create a box shape as an obstacle or add it to your tensegrity.
Constains definition of concrete class tgCompoundRigidSensor.
Utility class for class casting and filtering collections by type.
tgCompoundRigidSensor(tgModel *pModel)
Contains the definition of class tgTags.
tgSenseable * m_pSens
Definition: tgSensor.h:102
virtual std::vector< std::string > getSensorData()
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition: tgModel.h:128
virtual std::vector< std::string > getSensorDataHeadings()
Constains the implementation of mixin class tgSenseable.