From d2cefc0d2fe6a089530f2227895fa5124bb70e23 Mon Sep 17 00:00:00 2001 From: mallorycdaly Date: Wed, 15 Jun 2016 15:55:37 -0700 Subject: [PATCH] First commit of mcdaly dev folder --- src/dev/CMakeLists.txt | 1 + src/dev/mcdaly/12BarTensegrity/12BarCube.yaml | 98 ++++++++++ .../mcdaly/12BarTensegrity/12BarOrient.yaml | 8 + src/dev/mcdaly/12BarTensegrity/App12Bar.cpp | 82 +++++++++ src/dev/mcdaly/12BarTensegrity/CMakeLists.txt | 23 +++ .../12BarTensegrity/RPLengthController.h | 91 +++++++++ .../RPLengthController_rand.cpp | 77 ++++++++ .../mcdaly/12BarTensegrity/tensionsensor.cpp | 173 ++++++++++++++++++ src/dev/mcdaly/CMakeLists.txt | 5 + 9 files changed, 558 insertions(+) create mode 100644 src/dev/mcdaly/12BarTensegrity/12BarCube.yaml create mode 100644 src/dev/mcdaly/12BarTensegrity/12BarOrient.yaml create mode 100644 src/dev/mcdaly/12BarTensegrity/App12Bar.cpp create mode 100644 src/dev/mcdaly/12BarTensegrity/CMakeLists.txt create mode 100644 src/dev/mcdaly/12BarTensegrity/RPLengthController.h create mode 100644 src/dev/mcdaly/12BarTensegrity/RPLengthController_rand.cpp create mode 100644 src/dev/mcdaly/12BarTensegrity/tensionsensor.cpp create mode 100644 src/dev/mcdaly/CMakeLists.txt diff --git a/src/dev/CMakeLists.txt b/src/dev/CMakeLists.txt index e26331be1..17ce9aaa2 100644 --- a/src/dev/CMakeLists.txt +++ b/src/dev/CMakeLists.txt @@ -17,5 +17,6 @@ subdirs( CPG_feedback eajung ezhu + mcdaly ) diff --git a/src/dev/mcdaly/12BarTensegrity/12BarCube.yaml b/src/dev/mcdaly/12BarTensegrity/12BarCube.yaml new file mode 100644 index 000000000..e2d28c5be --- /dev/null +++ b/src/dev/mcdaly/12BarTensegrity/12BarCube.yaml @@ -0,0 +1,98 @@ +nodes: + n0: [-1.85, 0, -.43] + n1: [-0.76, -3.69, 0.65] + n2: [-1.85, 0, -1.96] + n3: [1.85, -1.08, -3.04] + n4: [-0.76, 0, -3.04] + n5: [-1.85, -3.69, -1.96] + n6: [0.76, 0, -3.04] + n7: [1.85, -1.08, 0.65] + n8: [1.85, 0, -1.96] + n9: [0.76, -3.69, -3.04] + n10: [1.85, 0, -.43] + n11: [-1.85, -1.08, 0.65] + n12: [0.76, 0, 0.65] + n13: [1.85, -3.69, -.43] + n14: [-.76, 0, 0.65] + n15: [-1.85, -1.08, -3.04] + n16: [-1.85, -3.69, -0.43] + n17: [1.85, -2.61, 0.65] + n18: [-0.76, -3.69, -3.04] + n19: [-1.85, -2.61, 0.65] + n20: [1.85, -3.69, -1.96] + n21: [-1.85, -2.61, -3.04] + n22: [0.76, -3.69, 0.65] + n23: [1.85, -2.61, -3.04] + +pair_groups: + rod: + - [n0, n1] + - [n2, n3] + - [n4, n5] + - [n6, n7] + - [n8, n9] + - [n10, n11] + - [n12, n13] + - [n14, n15] + - [n16, n17] + - [n18, n19] + - [n20, n21] + - [n22, n23] + + string: + - [n0, n2] + - [n0, n14] + - [n0, n11] + - [n2, n4] + - [n2, n15] + - [n4, n15] + - [n4, n6] + - [n6, n8] + - [n6, n3] + - [n8, n3] + - [n3, n23] + - [n8, n10] + - [n10, n12] + - [n10, n7] + - [n7, n12] + - [n14, n12] + - [n14, n11] + - [n23, n20] + - [n9, n23] + - [n7, n17] + - [n17, n13] + - [n13, n22] + - [n22, n17] + - [n22, n1] + - [n1, n16] + - [n16, n19] + - [n19, n1] + - [n19, n11] + - [n5, n18] + - [n18, n21] + - [n21, n5] + - [n21, n15] + - [n18, n9] + - [n9, n20] + - [n20, n13] + - [n16, n5] + +builders: + rod: + class: tgRodInfo + parameters: + density: 0.688 + radius: 0.21 + friction: 0.99 + roll_friction: 0.01 + restitution: 0.0 + + string: + class: tgBasicActuatorInfo + parameters: + stiffness: 800.0 + damping: 60.0 + pretension: 400.0 + history: 0 + max_tension: 3000 + target_velocity: 10 \ No newline at end of file diff --git a/src/dev/mcdaly/12BarTensegrity/12BarOrient.yaml b/src/dev/mcdaly/12BarTensegrity/12BarOrient.yaml new file mode 100644 index 000000000..aa46a771d --- /dev/null +++ b/src/dev/mcdaly/12BarTensegrity/12BarOrient.yaml @@ -0,0 +1,8 @@ +substructures: + 12BarCube: + path: ./12BarCube.yaml + translation: [0, 25, 0] + rotation: + axis: [1, 1, 0] + angle: 0 + scale: 2 diff --git a/src/dev/mcdaly/12BarTensegrity/App12Bar.cpp b/src/dev/mcdaly/12BarTensegrity/App12Bar.cpp new file mode 100644 index 000000000..61edfe539 --- /dev/null +++ b/src/dev/mcdaly/12BarTensegrity/App12Bar.cpp @@ -0,0 +1,82 @@ +/* + * 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 App6Bar.cpp + * @brief Contains the definition function main() for App6Bar + * which builds a 6 bar tensegrity structure defined in YAML + * @author Edward Zhu + * $Id$ + */ + +// This application +#include "../../../yamlbuilder/TensegrityModel.h" +#include "RPLengthController.h" +// This library +#include "core/terrain/tgBoxGround.h" +#include "core/tgModel.h" +#include "core/tgSimulation.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgWorld.h" +// Bullet Physics +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include + +/** + * 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) +{ + // 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); + + const tgWorld::Config config(98.1); // gravity, dm/sec^2 + tgWorld world(config, ground); + + // create the view + const double timestep_physics = 0.001; // seconds + 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 + TensegrityModel* const myModel = new TensegrityModel(argv[1]); + + RPLengthController* const tension_sensor = new RPLengthController(); + myModel -> attach(tension_sensor); + + // Add the model to the world + simulation.addModel(myModel); + + simulation.run(); + + // teardown is handled by delete + return 0; +} diff --git a/src/dev/mcdaly/12BarTensegrity/CMakeLists.txt b/src/dev/mcdaly/12BarTensegrity/CMakeLists.txt new file mode 100644 index 000000000..1374b862a --- /dev/null +++ b/src/dev/mcdaly/12BarTensegrity/CMakeLists.txt @@ -0,0 +1,23 @@ +link_directories(${LIB_DIR}) + +link_libraries(core + tgcreator + controllers + util + sensors + terrain + tgOpenGLSupport + yaml-cpp + Adapters + TensegrityModel) + +# add_library(TensegrityModel +# TensegrityModel.cpp) + +add_executable(App12Bar +# TensegrityModel.cpp + App12Bar.cpp + RPLengthController.h + RPLengthController_rand.cpp) + + diff --git a/src/dev/mcdaly/12BarTensegrity/RPLengthController.h b/src/dev/mcdaly/12BarTensegrity/RPLengthController.h new file mode 100644 index 000000000..f5d633e96 --- /dev/null +++ b/src/dev/mcdaly/12BarTensegrity/RPLengthController.h @@ -0,0 +1,91 @@ +/* + * 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. +*/ + +#ifndef RP_LENGTH_CONTROLLER_H +#define RP_LENGTH_CONTROLLER_H + +/** + * @file RPTensionController.h + * @brief Contains the definition of class RPTensionController. + * @author Brian Tietz + * @version 1.0.0 + * $Id$ + */ + +// This library +#include "core/tgObserver.h" +#include "controllers/tgBasicController.h" + #include "core/tgBasicActuator.h" + +// The C++ Standard Library +#include + +// Forward declarations +class TensegrityModel; + +#include "core/abstractMarker.h" //needed in order to fetch node positions + +/** + * A controller to apply uniform tension to a RPModel. Iterates through + * all tgLinearString members and calls tensionMinLengthController + */ +class RPLengthController : public tgObserver +{ +public: + + /** + * Construct a RPTensionController. + * @param[in] tension, a double specifying the desired tension + * throughougt structure. Must be non-negitive + */ + RPLengthController(const double length = 400); + + /** + * Nothing to delete, destructor must be virtual + */ + virtual ~RPLengthController(); + + virtual void onSetup(TensegrityModel& subject); + + /** + * Apply the tension controller. Called my notifyStep(dt) of its + * subject. The tgLinearStrings will update using + * their tensionMinLengthController each step + * @param[in] subject - the RPModel that is being controlled. Must + * have a list of allMuscles populated + * @param[in] dt, current timestep must be positive + */ + virtual void onStep(TensegrityModel& subject, double dt); + + std::vector m_controllers; + std::vector rand_lengths; + std::vector actuators; + +private: + + /** + * The tension setpoint that will be passed to the muscles. Set + * in the constructor + */ + const double m_length; + double globalTime; + int toggle; + +}; + +#endif // RP_LENGTH_CONTROLLER_H diff --git a/src/dev/mcdaly/12BarTensegrity/RPLengthController_rand.cpp b/src/dev/mcdaly/12BarTensegrity/RPLengthController_rand.cpp new file mode 100644 index 000000000..205c46601 --- /dev/null +++ b/src/dev/mcdaly/12BarTensegrity/RPLengthController_rand.cpp @@ -0,0 +1,77 @@ +/* + * 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 RPTensionController.cpp + * @brief Implementation of six strut tensegrity. + * @author Brian Tietz + * @version 1.0.0 + * $Id$ + */ + +#include +// This module +#include "RPLengthController.h" +// This application +#include "../../../yamlbuilder/TensegrityModel.h" +// This library +#include "core/tgBasicActuator.h" +#include "core/tgCast.h" +// The C++ Standard Library +#include +#include +#include + +using namespace std; + +RPLengthController::RPLengthController(const double length) : + m_length(length) +{ + if (length < 0.0) + { + throw std::invalid_argument("Negative length"); + } +} + +RPLengthController::~RPLengthController() +{ +} + +void RPLengthController::onSetup(TensegrityModel& subject) +{ + + actuators = subject.getAllActuators(); + +} + +void RPLengthController::onStep(TensegrityModel& subject, double dt) +{ + if (dt <= 0.0) + { + throw std::invalid_argument("dt is not positive"); + } + else{ + for(int k=0;kgetTension() << ", "; + std::cout << actuators[k]->getTension() << ", "; + } + std::cout << std::endl; + } +} + + diff --git a/src/dev/mcdaly/12BarTensegrity/tensionsensor.cpp b/src/dev/mcdaly/12BarTensegrity/tensionsensor.cpp new file mode 100644 index 000000000..ade367079 --- /dev/null +++ b/src/dev/mcdaly/12BarTensegrity/tensionsensor.cpp @@ -0,0 +1,173 @@ +/* + * 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 RPModel.cpp + * @brief Contains the implementation of class RocketPower. + * @author Brian Cera, based on code from Kyunam Kim + * @version 1.0.0 + * $Id$ + */ + +// This module +//#include "RPModel.h" +// This library +#include "core/tgBasicActuator.h" +#include "core/tgRod.h" +#include "tgcreator/tgBuildSpec.h" +#include "tgcreator/tgBasicActuatorInfo.h" +#include "tgcreator/tgRodInfo.h" +#include "tgcreator/tgStructure.h" +#include "tgcreator/tgStructureInfo.h" +// The Bullet Physics library +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include +#include +#include + +namespace +{ + // see tgBasicActuator and tgRod for a descripton of these rod parameters + // (specifically, those related to the motor moving the strings.) + // NOTE that any parameter that depends on units of length will scale + // with the current gravity scaling. E.g., with gravity as 98.1, + // the length units below are in decimeters. + + // Note: This current model of the SUPERball rod is 1.5m long by 3 cm radius, + // which is 0.00424 m^3. + // For SUPERball v1.5, mass = 3.5kg per strut, which comes out to + // 0.825 kg / (decimeter^3). + + // similarly, frictional parameters are for the tgRod objects. + + //const double sf = 20;//scaling factor with respect to meter scale. E.g., centimeter scale is achieved by setting sf = 100 + //const double length_scale = 0.25; //1 makes 4 m long rods + // In meter scale, the robot is too small, while in centimeter scale, the robot rotates freely (free energy!) + // Also, don't forget to change gravity scale in AppThruster.cpp and T6Thruster.cpp! + + const struct Config + { + double density; + double radius; + double stiffness; + double damping; + double rod_length; + double rod_space; + double friction; + double rollFriction; + double restitution; + double pretension; + bool hist; + double maxTens; + double targetVelocity; + } + c = + { + 2700/pow(sf,3),//0.688, // density (kg / length^3) + 0.0254*sf,//0.31, // radius (length) + 600,//1192.5*10,//613.0, // stiffness (kg / sec^2) was 1500 + 500, // damping (kg / sec) + 4*sf*length_scale, // rod_length (length) + .02*sf, // rod_space (length) + 0.99, // friction (unitless) + 0.1, // rollFriction (unitless) + 0.0, // restitution (?) + 150*sf, //610, // pretension -> set to 4 * 613, the previous value of the rest length controller + 0, // History logging (boolean) + 300*sf, // maxTens + .02, //sf, // targetVelocity + + // Use the below values for earlier versions of simulation. + // 1.006, + // 0.31, + // 300000.0, + // 3000.0, + // 15.0, + // 7.5, + }; + +} // namespace + +RPModel::RPModel() : tgModel() +{ +} + +RPModel::~RPModel() +{ +} + +void RPModel::setup(tgWorld& world) +{ + + allAbstractMarkers=tgCast::filter (getDescendants()); + +} + +void RPModel::step(double dt) +{ + // Precondition + if (dt <= 0.0) + { + throw std::invalid_argument("dt is not positive"); + } + else + { + // Notify observers (controllers) of the step so that they can take action + notifyStep(dt); + tgModel::step(dt); // Step any children + } + for(int k=1;k<36;k++){ + std::cout << allActuators[k]->getTension() << " "; + } + std::cout << std::endl; + +} + + +void RPModel::onVisit(tgModelVisitor& r) +{ + tgModel::onVisit(r); +} + +const std::vector& RPModel::getAllActuators() const +{ + return allActuators; +} + + +const std::vector& RPModel::getAllRods() const +{ + return allRods; +} + +const std::vector& RPModel::getAllBaseRigids() const +{ + return allBaseRigids; +} + +const std::vector& RPModel::getAllAbstractMarkers() const +{ + return allAbstractMarkers; +} + +void RPModel::teardown() +{ + notifyTeardown(); + tgModel::teardown(); +} diff --git a/src/dev/mcdaly/CMakeLists.txt b/src/dev/mcdaly/CMakeLists.txt new file mode 100644 index 000000000..a5a30383d --- /dev/null +++ b/src/dev/mcdaly/CMakeLists.txt @@ -0,0 +1,5 @@ +Project(mcdaly) + +subdirs( + 12BarTensegrity + )