3 #include "ParamHandler.hpp" 5 #include <Configuration.h> 20 : _simParams(params), _userParams(userParams), _tau(12) {
22 printf(
"[Simulation] Load parameters...\n");
27 "[ERROR] Simulator parameters are not fully initialized. You forgot: " 30 throw std::runtime_error(
"simulator not initialized");
35 printf(
"[Simulation] Setup LCM...\n");
38 printf(
"[ERROR] Failed to set up LCM\n");
39 throw std::runtime_error(
"lcm bad");
44 printf(
"[Simulation] Build quadruped...\n");
47 : buildCheetah3<double>();
48 printf(
"[Simulation] Build actuator model...\n");
54 printf(
"[Simulation] Setup Cheetah graphics...\n");
56 truthColor << 0.2, 0.4, 0.2, 0.6;
57 seColor << 0.4, 0.2, 0.2, 0.6;
66 printf(
"[Simulation] Build rigid body model...\n");
74 for (
u32 i = 0; i < 12; i++) {
155 printf(
"[Simulation] Setup low-level control...\n");
158 for (
int leg = 0; leg < 4; leg++) {
167 for (
int leg = 0; leg < 4; leg++) {
181 printf(
"[Simulation] Setup shared memory...\n");
191 printf(
"[Simulation] Load control parameters...\n");
203 printf(
"Not all robot control parameters were initialized. Missing:\n%s\n",
205 throw std::runtime_error(
"not all parameters initialized from ini file");
209 printf(
"[Simulation] Setup IMU simulator...\n");
213 printf(
"[Simulation] Ready!\n");
232 strcpy(request.
name, name.c_str());
233 request.
value = value;
235 printf(
"%s\n", request.
toString().c_str());
249 "[ERROR] Timed out waiting for message from robot! Did it crash?\n");
261 assert(std::string(response.
name) == request.
name);
274 printf(
"[Simulation] Waiting for robot...\n");
285 printf(
"Success! the robot is alive\n");
290 printf(
"[Simulation] Send robot control parameters to robot...\n");
293 kv.second->_kind,
false);
298 kv.second->_kind,
true);
306 double dtHighLevelControl) {
315 #ifndef DISABLE_HIGH_LEVEL_CONTROL 324 for (
int leg = 0; leg < 4; leg++) {
325 for (
int joint = 0; joint < 3; joint++) {
332 for (
int leg = 0; leg < 4; leg++) {
333 for (
int joint = 0; joint < 3; joint++) {
363 for (
int leg = 0; leg < 4; leg++) {
380 for (
int leg = 0; leg < 4; leg++) {
381 for (
int joint = 0; joint < 3; joint++) {
391 tiBoard.run_ti_board_iteration();
418 for (
int i = 0; i < 4; i++) {
448 "[ERROR] Timed out waiting for message from robot! Did it crash?\n");
462 for (
int i = 0; i < 4; i++) {
480 Vec3<double> omega = Rbody.transpose() * state.bodyVelocity.head<3>();
481 Vec3<double> v = Rbody.transpose() * state.bodyVelocity.tail<3>();
483 for (
size_t i = 0; i < 4; i++) {
484 _simLCM.quat[i] = state.bodyOrientation[i];
487 for (
size_t i = 0; i < 3; i++) {
488 _simLCM.vb[i] = state.bodyVelocity[i + 3];
490 for (
size_t j = 0; j < 3; j++) {
493 _simLCM.omegab[i] = state.bodyVelocity[i];
495 _simLCM.p[i] = state.bodyPosition[i];
497 _simLCM.vbd[i] = dstate.dBodyVelocity[i + 3];
500 for (
size_t leg = 0; leg < 4; leg++) {
501 for (
size_t joint = 0; joint < 3; joint++) {
502 _simLCM.q[leg][joint] = state.q[leg * 3 + joint];
503 _simLCM.qd[leg][joint] = state.qd[leg * 3 + joint];
504 _simLCM.qdd[leg][joint] = dstate.qdd[leg * 3 + joint];
521 double sizeX,
double sizeY,
double checkerX,
522 double checkerY,
bool addToWindow) {
547 double width,
double height,
562 bool addToWindow,
bool transparent) {
588 u64 desiredSteps = 0;
591 double frameTime = 1. / 60.;
592 double lastSimTime = 0;
595 "[Simulator] Starting run loop (dt %f, dt-low-level %f, dt-high-level %f " 596 "speed %f graphics %d)...\n",
602 double dtLowLevelControl =
_simParams.low_level_dt;
603 double dtHighLevelControl =
_simParams.high_level_dt;
608 step(dt, dtLowLevelControl, dtHighLevelControl);
612 double timeRemaining = frameTime - frameTimer.
getSeconds();
613 if (timeRemaining > 0) {
614 usleep((
u32)(timeRemaining * 1e6));
618 double realElapsedTime = frameTimer.
getSeconds();
624 "[Simulation Run %5.2fx]\n" 633 desiredSteps += nStepsPerFrame;
640 printf(
"load terrain %s\n", terrainFileName.c_str());
641 ParamHandler paramHandler(terrainFileName);
643 if (!paramHandler.fileOpenedSuccessfully()) {
644 printf(
"[ERROR] could not open yaml file for terrain\n");
645 throw std::runtime_error(
"yaml bad");
648 std::vector<std::string> keys = paramHandler.getKeys();
650 for (
auto& key : keys) {
651 auto load = [&](
double& val,
const std::string& name) {
652 if (!paramHandler.getValue<
double>(key, name, val))
653 throw std::runtime_error(
"terrain read bad: " + key +
" " + name);
656 auto loadVec = [&](
double& val,
const std::string& name,
size_t idx) {
657 std::vector<double> v;
658 if (!paramHandler.getVector<
double>(key, name, v))
659 throw std::runtime_error(
"terrain read bad: " + key +
" " + name);
663 auto loadArray = [&](
double* val,
const std::string& name,
size_t idx) {
664 std::vector<double> v;
665 if (!paramHandler.getVector<
double>(key, name, v))
666 throw std::runtime_error(
"terrain read bad: " + key +
" " + name);
667 assert(v.size() == idx);
668 for (
size_t i = 0; i < idx; i++) val[i] = v[i];
671 printf(
"terrain element %s\n", key.c_str());
672 std::string typeName;
673 paramHandler.getString(key,
"type", typeName);
674 if (typeName ==
"infinite-plane") {
675 double mu, resti, height, gfxX, gfxY, checkerX, checkerY;
677 load(resti,
"restitution");
678 load(height,
"height");
679 loadVec(gfxX,
"graphicsSize", 0);
680 loadVec(gfxY,
"graphicsSize", 1);
681 loadVec(checkerX,
"checkers", 0);
682 loadVec(checkerY,
"checkers", 1);
685 }
else if (typeName ==
"box") {
686 double mu, resti, depth, width, height, transparent;
690 load(resti,
"restitution");
691 load(depth,
"depth");
692 load(width,
"width");
693 load(height,
"height");
694 loadArray(pos,
"position", 3);
695 loadArray(ori,
"orientation", 3);
696 load(transparent,
"transparent");
699 R_box.transposeInPlace();
702 addGraphics, transparent != 0.);
703 }
else if (typeName ==
"stairs") {
704 double mu, resti, rise, run, stepsDouble, width, transparent;
708 load(resti,
"restitution");
710 load(width,
"width");
712 load(stepsDouble,
"steps");
713 loadArray(pos,
"position", 3);
714 loadArray(ori,
"orientation", 3);
715 load(transparent,
"transparent");
719 R.transposeInPlace();
721 size_t steps = (size_t)stepsDouble;
723 double heightOffset = rise / 2;
724 double runOffset = run / 2;
730 addGraphics, transparent != 0.);
732 heightOffset += rise / 2;
735 }
else if (typeName ==
"mesh") {
736 double mu, resti, transparent, grid;
738 std::vector<std::vector<double> > height_map_2d;
740 load(resti,
"restitution");
741 load(transparent,
"transparent");
743 loadVec(left_corner[0],
"left_corner_loc", 0);
744 loadVec(left_corner[1],
"left_corner_loc", 1);
745 loadVec(left_corner[2],
"left_corner_loc", 2);
749 bool file_input(
false);
750 paramHandler.getBoolean(key,
"heightmap_file", file_input);
753 std::string file_name;
754 paramHandler.getString(key,
"heightmap_file_name", file_name);
755 std::ifstream f_height;
756 f_height.open(THIS_COM
"/config/" + file_name);
757 if (!f_height.good()) {
758 std::cout <<
"file reading error: " 759 << THIS_COM
"../config/" + file_name << std::endl;
766 std::vector<double> height_map_vec;
767 while (getline(f_height, line)) {
768 std::istringstream iss(line);
771 height_map_vec.push_back(tmp);
775 height_map_2d.push_back(height_map_vec);
776 height_map_vec.clear();
783 paramHandler.get2DArray(key,
"height_map", height_map_2d);
784 x_len = height_map_2d.size();
785 y_len = height_map_2d[0].size();
790 for (
int i(0); i < x_len; ++i) {
791 for (
int j(0); j < y_len; ++j) {
792 height_map(i, j) = height_map_2d[i][j];
800 throw std::runtime_error(
"unknown terrain " + typeName);
807 _sharedMemory().robotToSim.mainCheetahVisualization.quat.cast<
double>();
809 _sharedMemory().robotToSim.mainCheetahVisualization.p.cast<
double>();
810 for (
int i = 0; i < 12; i++)
void addCollisionBox(double mu, double resti, double depth, double width, double height, const Vec3< double > &pos, const Mat3< double > &ori, bool addToWindow=true, bool transparent=true)
void addMesh(double grid_size, const Vec3< double > &left_corner, const DMat< double > &height_map, bool transparent)
VisualizationData * _visualizationData
void runAtSpeed(bool graphics=true)
void step(double dt, double dtLowLevelControl, double dtHighLevelControl)
void forwardKinematics()
Do forward kinematics for feet.
ControlParameterCollection collection
Mat3< T > coordinateRotation(CoordinateAxis axis, T theta)
ControlParameterValueKind parameterKind
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3< T > position
ControlParameterValueKind
void updateCheaterState(const FBModelState< T > &robotState, const FBModelStateDerivative< T > &robotStateD, CheaterState< T > &state)
Mat3< typename T::Scalar > rpyToRotMat(const Eigen::MatrixBase< T > &v)
std::map< std::string, ControlParameter * > _map
void addCollisionMesh(double mu, double resti, double grid_size, const Vec3< double > &left_corner_loc, const DMat< double > &height_map, bool addToWindow=true, bool transparent=true)
std::vector< ActuatorModel< double > > _actuatorModels
typename Eigen::Matrix< T, 3, 3 > Mat3
Data structure containing parameters for quadruped robot.
#define MINI_CHEETAH_DEFAULT_PARAMETERS
void reset_ti_board_data()
typename Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > DMat
size_t setupMiniCheetah(Vec4< float > color, bool useOld)
bool isFullyInitialized()
typename Eigen::Matrix< T, 3, 1 > Vec3
TI_BoardControl _tiBoards[4]
ControlParameterRequestKind requestKind
const Vec3< T > & getContactForce(size_t idx)
SharedMemoryObject< SimulatorSyncronizedMessage > _sharedMemory
DynamicsSimulator< double > * _robotDataSimulator
void updateRobotFromModel(DynamicsSimulator< T > &model, size_t id, bool updateOrigin=false)
void init(float side_sign, s32 board)
void loadTerrainFile(const std::string &terrainFileName, bool addGraphics=true)
void setState(const FBModelState< T > &state)
RobotControlParameters _robotParams
char name[CONTROL_PARAMETER_MAXIMUM_NAME_LENGTH]
void set_link_lengths(float l1, float l2, float l3)
std::string getLcmUrl(s64 ttl)
void reset_ti_board_command()
size_t setupCheetah3(Vec4< float > color, bool useOld)
void addBox(double depth, double width, double height, const Vec3< double > &pos, const Mat3< double > &ori, bool transparent)
std::string getConfigDirectoryPath()
void addCollisionPlane(double mu, double resti, double height, double sizeX=20, double sizeY=20, double checkerX=40, double checkerY=40, bool addToWindow=true)
ControlParameterValueKind parameterKind
void addCollisionMesh(T mu, T rest, T grid_size, const Vec3< T > &left_corner_loc, const DMat< T > &height_map)
Quadruped< double > _quadruped
#define DEVELOPMENT_SIMULATOR_SHARED_MEMORY_NAME
void setRobotState(FBModelState< double > &state)
double _timeOfNextLowLevelControl
bool createNew(const std::string &name, bool allowOverwrite=false)
void updateVectornav(const FBModelState< T > &robotState, const FBModelStateDerivative< T > &robotStateD, VectorNavData *data)
Vec3< typename T::Scalar > quatToRPY(const Eigen::MatrixBase< T > &q)
SpineBoard _spineBoards[4]
void setHoming(const RobotHomingInfo< T > &homing)
void init(float side_sign)
ControlParameters & _userParams
SimulatorControlParameters & _simParams
Code to read the Logitech F310 Game Controller Creates a DriverCommand object to be sent to the robot...
#define CHEETAH_3_DEFAULT_PARAMETERS
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Simulation(RobotType robot, Graphics3D *window, SimulatorControlParameters ¶ms, ControlParameters &userParams)
FloatingBaseModel< double > _model
Quat< typename T::Scalar > rotationMatrixToQuaternion(const Eigen::MatrixBase< T > &r1)
typename Eigen::Matrix< T, 4, 1 > Vec4
char name[CONTROL_PARAMETER_MAXIMUM_NAME_LENGTH]
void step(T dt, const DVec< T > &tau, T kp, T kd)
Initialize simulator with given model.
ControlParameterValue value
GamepadCommand & getDriverCommand()
const FBModelState< T > & getState() const
void updateCheckerboard(T height, size_t id)
size_t _controllerRobotID
void addCollisionPlane(T mu, T rest, T height)
vector< uint64_t > _footIndicesGC
const FBModelStateDerivative< T > & getDState() const
DynamicsSimulator< double > * _simulator
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
ImuSimulator< double > * _imuSimulator
void updateAdditionalInfo(DynamicsSimulator< T > &model)
Mat3< typename T::Scalar > quaternionToRotationMatrix(const Eigen::MatrixBase< T > &q)
void run_ti_board_iteration()
std::vector< ActuatorModel< T > > buildActuatorModels()
FBModelState< double > _robotControllerState
void initializeFromYamlFile(const std::string &path)
const FloatingBaseModel< T > & getModel()
double _timeOfNextHighLevelControl
typename Eigen::Matrix< T, 3, 3 > RotMat
void sendControlParameter(const std::string &name, ControlParameterValue value, ControlParameterValueKind kind, bool isUser)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Quat< T > bodyOrientation
std::string generateUnitializedList()
size_t addCheckerboard(Checkerboard &checkerBoard)
FloatingBaseModel< T > buildModel()
void addCollisionBox(T mu, T rest, T depth, T width, T height, const Vec3< T > &pos, const Mat3< T > &ori)
FloatingBaseModel< double > _robotDataModel
MX f(const MX &x, const MX &u)