CordeModel.h

Go to the documentation of this file.

/*
 * Copyright © 2012, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 * 
 * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
 * under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0.
 * 
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
*/

//#define BT_USE_DOUBLE_PRECISION

#ifndef CORDE_MODEL
#define CORDE_MODEL

// Bullet Linear Algebra
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h"

// The C++ Standard Library
#include <vector>

class CordeModel
{
public:
                struct Config
                {
                                Config(const std::size_t res,
                                                                const double r, const double d,
                                                                const double ym, const double shm,
                                                                const double stm, const double csc,
                                                                const double gt, const double gr);
                                
                                const std::size_t resolution;
                                const double radius;
                                const double density;
                                const double YoungMod;
                                const double ShearMod;
                                const double StretchMod;
                                const double ConsSpringConst;
                                const double gammaT;
                                const double gammaR;
                };
                
                CordeModel(btVector3 pos1, btVector3 pos2, btQuaternion quat1, btQuaternion quat2, CordeModel::Config& Config);
                
                ~CordeModel();
                
                void step (btScalar dt);
                
private:
                void computeConstants();

                void stepPrerequisites();

                void computeInternalForces();
                
                void unconstrainedMotion(double dt);
                
                struct CordePositionElement
                {
                                CordePositionElement(btVector3 p1, double m);
                                
                                btVector3 pos;
                                btVector3 vel;
                                btVector3 force;
                                double mass;
                };
                
                struct CordeQuaternionElement
                {               
                                CordeQuaternionElement(btQuaternion q1);
                                
                                void transposeTorques();
                                void updateQDot();
                                
                                btQuaternion q;
                                btQuaternion qdot;
                                btQuaternion tprime;
                                btVector3 torques;
                                btVector3 omega;
                };
                
                CordeModel::Config m_config;
                
                std::vector<CordePositionElement*> m_massPoints;
                std::vector<CordeQuaternionElement*> m_centerlines;
                std::vector<double> linkLengths;
                std::vector<double> quaternionShapes;
                
                std::vector<double> computedStiffness;
                
                btVector3 computedInertia;
                btVector3 inverseInertia;
                
                bool invariant();
                
                double simTime;
};
 
 
#endif // CORDE_MODEL