Permalink
Browse files

Merged in tgCompoundRigidSensor from laika branch. Cleaned up sensors…

…-redesign infrastructure to accomodate for tgCompoundRigidSensor. Added compound body hash tag inside tgRigidAutoCompound. Added SpineKinematicsTest as an example of using tgCompoundRigidSensor.
  • Loading branch information...
apsabelhaus committed Feb 3, 2017
1 parent aef1998 commit 2b321010be50cb069cd7bd9163eafe2e78090d4f
Showing with 1,715 additions and 63 deletions.
  1. +1 −0 src/dev/ultra-spine/CMakeLists.txt
  2. +148 −0 src/dev/ultra-spine/SpineKinematicsTest/AppSpineKinematicsTest.cpp
  3. +21 −0 src/dev/ultra-spine/SpineKinematicsTest/CMakeLists.txt
  4. +152 −0 src/dev/ultra-spine/SpineKinematicsTest/SpineKinematicsTestController.cpp
  5. +130 −0 src/dev/ultra-spine/SpineKinematicsTest/SpineKinematicsTestController.h
  6. +119 −0 src/dev/ultra-spine/SpineKinematicsTest/SpineOnlyTest.yaml
  7. +145 −0 src/dev/ultra-spine/SpineKinematicsTest/TetrahedralSpineKinematicsTest.yaml
  8. +23 −0 src/dev/ultra-spine/SpineKinematicsTest/TetrahedronSymmetric.yaml
  9. +30 −0 src/dev/ultra-spine/SpineKinematicsTest/TetrahedronSymmetricFixed.yaml
  10. +45 −0 src/dev/ultra-spine/SpineKinematicsTest/TwoSegSpine.yaml
  11. +9 −1 src/sensors/CMakeLists.txt
  12. BIN src/sensors/{SensorsRedesignOutline_2017-01-03.docx → SensorsRedesignOngoingWork_Jan2017.docx}
  13. +223 −0 src/sensors/tgCompoundRigidSensor.cpp
  14. +109 −0 src/sensors/tgCompoundRigidSensor.h
  15. +241 −0 src/sensors/tgCompoundRigidSensorInfo.cpp
  16. +126 −0 src/sensors/tgCompoundRigidSensorInfo.h
  17. +33 −17 src/sensors/tgDataLogger2.cpp
  18. +18 −2 src/sensors/tgDataLogger2.h
  19. +16 −5 src/sensors/tgDataManager.cpp
  20. +1 −1 src/sensors/tgDataManager.h
  21. +1 −1 src/sensors/tgRodSensor.cpp
  22. +6 −3 src/sensors/tgRodSensorInfo.cpp
  23. +7 −1 src/sensors/tgRodSensorInfo.h
  24. +19 −5 src/sensors/tgSensorInfo.h
  25. +6 −3 src/sensors/tgSpringCableActuatorSensorInfo.cpp
  26. +7 −1 src/sensors/tgSpringCableActuatorSensorInfo.h
  27. +2 −1 src/tgcreator/CMakeLists.txt
  28. +1 −1 src/tgcreator/tgBoxMoreAnchorsInfo.h
  29. +56 −3 src/tgcreator/tgRigidAutoCompound.cpp
  30. +20 −3 src/tgcreator/tgRigidAutoCompound.h
  31. +0 −15 src/tgcreator/tgStructureInfo.cpp
@@ -4,4 +4,5 @@ subdirs(
VerticalSpine
VerticalSpine_CableCollision
HorizontalSpine
SpineKinematicsTest
)
@@ -0,0 +1,148 @@
/*
* 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.
*/
/**
* @file AppSpineKinematicsTest.cpp
* @brief Contains the definition function main() for AppSpineKinematicsTest
* which tests out some kinematic movements of a horizontal spine
* @author Drew Sabelhaus
* $Id$
*/
// This application
#include "yamlbuilder/TensegrityModel.h"
#include "SpineKinematicsTestController.h"
// This library
#include "core/terrain/tgBoxGround.h"
#include "core/tgModel.h"
#include "core/tgSimulation.h"
#include "core/tgSimViewGraphics.h"
#include "core/tgWorld.h"
#include "sensors/tgDataLogger2.h"
#include "sensors/tgRodSensorInfo.h"
#include "sensors/tgSpringCableActuatorSensorInfo.h"
#include "sensors/tgCompoundRigidSensorInfo.h"
// Bullet Physics
#include "LinearMath/btVector3.h"
// The C++ Standard Library
#include <iostream>
#include <string>
#include <vector>
/**
* The entry point.
* @param[in] argc the number of command-line arguments
* @param[in] argv argv[0] is the executable name
* @param[in] argv argv[1] is the path of the YAML encoded structure
* @return 0
*/
int main(int argc, char** argv)
{
// For this YAML parser app, need to check that an argument path was
// passed in.
if (argv[1] == NULL)
{
throw std::invalid_argument("No arguments passed in to the application. You need to specify which YAML file you wouldd like to build.");
}
// create the ground and world. Specify ground rotation in radians
const double yaw = 0.0;
const double pitch = 0.0;
const double roll = 0.0;
const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll));
// the world will delete this
tgBoxGround* ground = new tgBoxGround(groundConfig);
// For this kinematics test, no gravity. We want a purely kinematic movement,
// as if the spine was floating in space.
const tgWorld::Config config(0.0); // gravity, dm/sec^2
tgWorld world(config, ground);
// create the view
const double timestep_physics = 0.0001; // seconds
//const double timestep_physics = 0.001;
const double timestep_graphics = 1.f/60.f; // seconds
tgSimViewGraphics view(world, timestep_physics, timestep_graphics);
// create the simulation
tgSimulation simulation(view);
// create the models with their controllers and add the models to the simulation
// This constructor for TensegrityModel takes the 'debugging' flag as the
// second argument.
TensegrityModel* const myModel = new TensegrityModel(argv[1],false);
// Attach a controller to the model, if desired.
// This is a controller that interacts with a generic TensegrityModel as
// built by the TensegrityModel file, BUT it only actually works
// with the specific HorizontalSpine YAML file.
// @TODO: should this throw an error when attached to a model that
// wasn't built with the HorizontalSpine YAML file?
// Parameters for the Horizontal Spine Controller are specified in that .h file,
// repeated here:
double startTime = 3.0;
double minLength = 0.8;
double rate = 0.25;
std::vector<std::string> tagsToControl;
// HF is the right horizontal set
// HL is the bottom horizontal set maybe?
// HB is the left horizontal set
// HR is the top horizontal set.
// BUT, something is wrong here. Probably Bullet's numerical problems.
tagsToControl.push_back("HR");
tagsToControl.push_back("HF");
//tagsToControl.push_back("HB");
// Call the constructor for the controller
SpineKinematicsTestController* const controller =
new SpineKinematicsTestController(startTime, minLength, rate, tagsToControl);
// Attach the controller to the model. Must happen before running the
// simulation.
myModel->attach(controller);
// Add the model to the world
simulation.addModel(myModel);
// Add sensors using the new sensing framework
// A string prefix for the filename
std::string log_filename = "~/NTRTsim_logs/AppSpineKinematicsTest";
// The time interval between sensor readings:
double timeInterval = 0.2;
// First, create the data manager
tgDataLogger2* myDataLogger = new tgDataLogger2(log_filename, timeInterval);
//std::cout << myDataLogger->toString() << std::endl;
// Then, add the model to the data logger
myDataLogger->addSenseable(myModel);
// Create sensor infos for all the types of sensors that the data logger
// will create.
//tgRodSensorInfo* myRodSensorInfo = new tgRodSensorInfo();
tgSpringCableActuatorSensorInfo* mySCASensorInfo =
new tgSpringCableActuatorSensorInfo();
tgCompoundRigidSensorInfo* myCRSensorInfo = new tgCompoundRigidSensorInfo();
// Attach the sensor infos to the data logger
//myDataLogger->addSensorInfo(myRodSensorInfo);
myDataLogger->addSensorInfo(mySCASensorInfo);
myDataLogger->addSensorInfo(myCRSensorInfo);
// Next, attach it to the simulation
simulation.addDataManager(myDataLogger);
simulation.run();
// teardown is handled by delete
return 0;
}
@@ -0,0 +1,21 @@
link_directories(${LIB_DIR})
link_libraries(core
tgcreator
util
sensors
terrain
tgOpenGLSupport
yaml-cpp
Adapters
TensegrityModel)
# add_library(TensegrityModel
# TensegrityModel.cpp)
add_executable(AppSpineKinematicsTest
SpineKinematicsTestController.cpp
AppSpineKinematicsTest.cpp
)
@@ -0,0 +1,152 @@
/*
* 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.
*/
/**
* @file SpineKinematicsTestController.cpp
* @brief Implementation of SpineKinematicsTestController.
* @author Drew Sabelhaus
* $Id$
*/
// This module
#include "SpineKinematicsTestController.h"
// This application
#include "yamlbuilder/TensegrityModel.h"
// This library
#include "core/tgBasicActuator.h"
#include "core/tgSpringCableActuator.h"
#include "core/tgString.h"
#include "core/tgTags.h"
// The C++ Standard Library
#include <cassert>
#include <stdexcept>
#include <vector>
#include <iostream>
#include "helpers/FileHelpers.h"
#include <stdexcept>
// Constructor assigns variables, does some simple sanity checks.
// Also, initializes the accumulator variable timePassed so that it can
// be incremented in onStep.
SpineKinematicsTestController::SpineKinematicsTestController(double startTime,
double minLength,
double rate,
std::vector<std::string> tagsToControl) :
m_startTime(startTime),
m_minLength(minLength),
m_rate(rate),
m_tagsToControl(tagsToControl),
m_timePassed(0.0)
{
// start time must be greater than or equal to zero
if( m_startTime < 0.0 ) {
throw std::invalid_argument("Start time must be greater than or equal to zero.");
}
// min length must be between 1 and 0
else if( m_minLength > 1 ) {
throw std::invalid_argument("minLength is a percent, must be less than 1. (100%)");
}
else if( m_minLength < 0.0) {
throw std::invalid_argument("minLength is a percent, must be greater than 0.");
}
// rate must be greater than zero
else if( rate < 0.0 ) {
throw std::invalid_argument("Rate cannot be negative.");
}
// @TODO: what checks to make on tags?
}
/**
* The initializeActuators method is call in onSetup to put pointers to
* specific actuators in the cablesWithTags array, as well as store the initial
* rest lengths in the initialRL map.
*/
void SpineKinematicsTestController::initializeActuators(TensegrityModel& subject,
std::string tag) {
//DEBUGGING
std::cout << "Finding cables with the tag: " << tag << std::endl;
// Pick out the actuators with the specified tag
std::vector<tgBasicActuator*> foundActuators = subject.find<tgBasicActuator>(tag);
std::cout << "The following cables were found and will be controlled: "
<< std::endl;
//Iterate through array and output strings to command line
for (std::size_t i = 0; i < foundActuators.size(); i ++) {
std::cout << foundActuators[i]->getTags() << std::endl;
// Also, add the rest length of the actuator at this time
// to the list of all initial rest lengths.
initialRL[foundActuators[i]->getTags()] = foundActuators[i]->getRestLength();
//DEBUGGING:
std::cout << "Cable rest length at t=0 is "
<< initialRL[foundActuators[i]->getTags()] << std::endl;
}
// Add this list of actuators to the full list. Thanks to:
// http://stackoverflow.com/questions/201718/concatenating-two-stdvectors
cablesWithTags.insert( cablesWithTags.end(), foundActuators.begin(),
foundActuators.end() );
}
/**
* For this controller, the onSetup method initializes the actuators,
* which means just store pointers to them and record their rest lengths.
* This method calls the helper initializeActuators.
*/
void SpineKinematicsTestController::onSetup(TensegrityModel& subject)
{
std::cout << "Setting up the HorizontalSpine controller." << std::endl;
// << "Finding cables with tags: " << m_tagsToControl
// << std::endl;
cablesWithTags = {};
// For all the strings in the list, call initializeActuators.
std::vector<std::string>::iterator it;
for( it = m_tagsToControl.begin(); it < m_tagsToControl.end(); it++ ) {
// Call the helper for this tag.
initializeActuators(subject, *it);
}
std::cout << "Finished setting up the controller." << std::endl;
}
void SpineKinematicsTestController::onStep(TensegrityModel& subject, double dt)
{
// First, increment the accumulator variable.
m_timePassed += dt;
// Then, if it's passed the time to start the controller,
if( m_timePassed > m_startTime ) {
// For each cable, check if its rest length is past the minimum,
// otherwise adjust its length according to m_rate and dt.
for (std::size_t i = 0; i < cablesWithTags.size(); i ++) {
double currRestLength = cablesWithTags[i]->getRestLength();
// Calculate the minimum rest length for this cable.
// Remember that m_minLength is a percent.
double minRestLength = initialRL[cablesWithTags[i]->getTags()] * m_minLength;
// If the current rest length is still greater than the minimum,
if( currRestLength > minRestLength ) {
// output a progress bar for the controller, to track when control occurs.
std::cout << "." << i;
// Then, adjust the rest length of the actuator itself, according to
// m_rate and dt.
double nextRestLength = currRestLength - m_rate * dt;
//DEBUGGING
//std::cout << "Next Rest Length: " << nextRestLength << std::endl;
cablesWithTags[i]->setControlInput(nextRestLength,dt);
}
}
}
}
Oops, something went wrong.

0 comments on commit 2b32101

Please sign in to comment.