12 _userParameters(userParameters) {
16 printf(
"[RobotInterface] Load parameters...\n");
28 printf(
"Not all robot control parameters were initialized. Missing:\n%s\n",
30 throw std::runtime_error(
"not all parameters initialized from ini file");
32 printf(
"[RobotInterface] Init LCM\n");
33 printf(
"[RobotInterface] Init graphics\n");
35 robotColor << 0.2, 0.2, 0.6, 0.6;
47 _lcm.subscribe(
"main_cheetah_visualization",
50 printf(
"[RobotInterface] Init dynamics\n");
52 : buildCheetah3<double>();
56 for (
u32 i = 0; i < 12; i++) {
65 const lcm::ReceiveBuffer *rbuf,
const std::string &chan,
66 const cheetah_visualization_lcmt *msg) {
69 for (
int i = 0; i < 3; i++) {
73 for (
int i = 0; i < 4; i++) {
77 for (
int i = 0; i < 12; i++) {
101 "[ERROR] trying to send control parameter while a send is in progress, " 116 printf(
"set %s to %s (%d)\n", name.c_str(),
125 std::unique_lock<std::mutex> lock(
_lcmMutex);
127 if (
_lcmCV.wait_for(lock, 100ms) == std::cv_status::no_timeout) {
132 "[RobotInterface] Failed to send parameter %s (iter %d) " 133 "wakeup %d bad? %d\n",
143 "[RobotInterface] Failed to send parameter %s (iter %d timed out), " 145 name.c_str(), iteration);
153 const lcm::ReceiveBuffer *rbuf,
const std::string &chan,
154 const control_parameter_respones_lcmt *msg) {
159 "[RobotInterface] Got a control parameter response when we weren't " 160 "expecting one, ignoring it!\n");
170 std::unique_lock<std::mutex> lock(
_lcmMutex);
180 printf(
"[RobotInterface] Send parameters to robot...\n");
183 kv.second->_kind,
false);
188 kv.second->_kind,
true);
194 printf(
"stopInterface\n");
197 printf(
"stopall done\n");
199 printf(
"lcmthread joined\n");
204 _lcm.handleTimeout(1000);
VisualizationData * _visualizationData
void get(gamepad_lcmt *lcmt)
void forwardKinematics()
Do forward kinematics for feet.
ControlParameterCollection collection
FloatingBaseModel< double > _model
void sendControlParameter(const std::string &name, ControlParameterValue value, ControlParameterValueKind kind, bool isUser)
ControlParameterValueKind
std::map< std::string, ControlParameter * > _map
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RobotInterface(RobotType robotType, Graphics3D *gfx, PeriodicTaskManager *tm, ControlParameters &userParameters)
#define MINI_CHEETAH_DEFAULT_PARAMETERS
size_t setupMiniCheetah(Vec4< float > color, bool useOld)
bool isFullyInitialized()
Utility function to build a Cheetah 3 Quadruped object.
Utility function to build a Mini Cheetah Quadruped object.
void updateRobotFromModel(DynamicsSimulator< T > &model, size_t id, bool updateOrigin=false)
void setState(const FBModelState< T > &state)
std::string getLcmUrl(s64 ttl)
size_t setupCheetah3(Vec4< float > color, bool useOld)
FBModelState< double > _fwdKinState
std::string getConfigDirectoryPath()
#define INTERFACE_LCM_NAME
void handleControlParameter(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const control_parameter_respones_lcmt *msg)
PeriodicTaskManager _taskManager
std::vector< QMatrix4x4 > _kinematicXform
std::string controlParameterValueToString(ControlParameterValue v, ControlParameterValueKind kind)
#define CHEETAH_3_DEFAULT_PARAMETERS
Quadruped< double > _quadruped
void handleVisualizationData(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const cheetah_visualization_lcmt *msg)
VisualizationData _visualizationData
#define ROBOT_INTERFACE_UPDATE_PERIOD
typename Eigen::Matrix< T, 4, 1 > Vec4
#define TIMES_TO_RESEND_CONTROL_PARAM
control_parameter_request_lcmt _parameter_request_lcmt
GamepadCommand & getDriverCommand()
void updateCheckerboard(T height, size_t id)
DynamicsSimulator< double > * _simulator
bool _pendingControlParameterSend
bool _waitingForLcmResponse
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
Types to allow remote access of control parameters, for use with LCM/Shared memory.
std::condition_variable _lcmCV
gamepad_lcmt _gamepad_lcmt
Interface between simulator and hardware using LCM.
RobotControlParameters _controlParameters
void initializeFromYamlFile(const std::string &path)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Quat< T > bodyOrientation
std::string generateUnitializedList()
size_t addCheckerboard(Checkerboard &checkerBoard)
FloatingBaseModel< T > buildModel()
ControlParameters & _userParameters