Cheetah Software  1.0
RobotInterface.cpp
Go to the documentation of this file.
1 #include "RobotInterface.h"
3 #include <Dynamics/Cheetah3.h>
4 #include <Dynamics/MiniCheetah.h>
5 #include <unistd.h>
7 
9  PeriodicTaskManager *tm, ControlParameters& userParameters)
10  : PeriodicTask(tm, ROBOT_INTERFACE_UPDATE_PERIOD, "robot-interface"),
11  _lcm(getLcmUrl(255)),
12  _userParameters(userParameters) {
13  _parameter_request_lcmt.requestNumber = 0;
14  _gfx = gfx;
16  printf("[RobotInterface] Load parameters...\n");
20  } else if (_robotType == RobotType::CHEETAH_3) {
23  } else {
24  assert(false);
25  }
26 
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");
31  }
32  printf("[RobotInterface] Init LCM\n");
33  printf("[RobotInterface] Init graphics\n");
34  Vec4<float> robotColor;
35  robotColor << 0.2, 0.2, 0.6, 0.6;
36  _robotID = _robotType == RobotType::MINI_CHEETAH ? gfx->setupMiniCheetah(robotColor, true)
37  : gfx->setupCheetah3(robotColor, true);
38  printf("draw list has %lu items\n", _gfx->_drawList._kinematicXform.size());
40  Checkerboard checker(10, 10, 10, 10);
41  uint64_t floorID = _gfx->_drawList.addCheckerboard(checker);
42  _gfx->_drawList.updateCheckerboard(0, floorID);
44 
45  _lcm.subscribe("interface_response", &RobotInterface::handleControlParameter,
46  this);
47  _lcm.subscribe("main_cheetah_visualization",
49 
50  printf("[RobotInterface] Init dynamics\n");
51  _quadruped = robotType == RobotType::MINI_CHEETAH ? buildMiniCheetah<double>()
52  : buildCheetah3<double>();
55  DVec<double> zero12(12);
56  for (u32 i = 0; i < 12; i++) {
57  zero12[i] = 0.;
58  }
59 
60  _fwdKinState.q = zero12;
61  _fwdKinState.qd = zero12;
62 }
63 
65  const lcm::ReceiveBuffer *rbuf, const std::string &chan,
66  const cheetah_visualization_lcmt *msg) {
67  (void)rbuf;
68  (void)chan;
69  for (int i = 0; i < 3; i++) {
70  _fwdKinState.bodyPosition[i] = msg->x[i];
71  }
72 
73  for (int i = 0; i < 4; i++) {
74  _fwdKinState.bodyOrientation[i] = msg->quat[i];
75  }
76 
77  for (int i = 0; i < 12; i++) {
78  _fwdKinState.q[i] = msg->q[i];
79  }
80 
83 }
84 
86  if (_gfx) {
88  _gfx->update();
91  }
92 }
93 
94 using namespace std::chrono_literals;
95 
96 void RobotInterface::sendControlParameter(const std::string &name,
98  ControlParameterValueKind kind, bool isUser) {
100  printf(
101  "[ERROR] trying to send control parameter while a send is in progress, "
102  "ignoring!\n");
103  return;
104  }
106  for (int iteration = 0; iteration < TIMES_TO_RESEND_CONTROL_PARAM;) {
107  // new message
108  _parameter_request_lcmt.requestNumber++;
109 
110  // message data
111  _parameter_request_lcmt.requestKind = isUser ?
113  strcpy((char *)_parameter_request_lcmt.name, name.c_str());
114  memcpy(_parameter_request_lcmt.value, &value, sizeof(value));
115  _parameter_request_lcmt.parameterKind = (s8)kind;
116  printf("set %s to %s (%d)\n", name.c_str(),
117  controlParameterValueToString(value, kind).c_str(), iteration);
118 
119  // send
120  _lcm.publish("interface_request", &_parameter_request_lcmt);
121 
122  // wait for response with timeout
123  _waitingForLcmResponse = true;
124  _lcmResponseBad = true;
125  std::unique_lock<std::mutex> lock(_lcmMutex);
126 
127  if (_lcmCV.wait_for(lock, 100ms) == std::cv_status::no_timeout) {
128  _waitingForLcmResponse = false;
129  // check it
131  printf(
132  "[RobotInterface] Failed to send parameter %s (iter %d) "
133  "wakeup %d bad? %d\n",
134  name.c_str(), iteration, _waitingForLcmResponse, _lcmResponseBad);
135  usleep(100000); // sleep a bit to let other bad sends happen
136  } else {
137  iteration++;
138  }
139  } else {
140  _waitingForLcmResponse = false;
141  // fail!
142  printf(
143  "[RobotInterface] Failed to send parameter %s (iter %d timed out), "
144  "trying again...\n",
145  name.c_str(), iteration);
146  usleep(100000); // sleep a bit to let other bad sends happen
147  }
148  }
150 }
151 
153  const lcm::ReceiveBuffer *rbuf, const std::string &chan,
154  const control_parameter_respones_lcmt *msg) {
155  (void)rbuf;
156  (void)chan;
157  if (!_waitingForLcmResponse) {
158  printf(
159  "[RobotInterface] Got a control parameter response when we weren't "
160  "expecting one, ignoring it!\n");
161  _lcmResponseBad = true;
162  return;
163  }
164 
165  // got a real response, let's check it
166  if (msg->requestNumber == _parameter_request_lcmt.requestNumber &&
167  msg->parameterKind == _parameter_request_lcmt.parameterKind &&
168  std::string((char *)msg->name) == (char *)_parameter_request_lcmt.name) {
169  _lcmResponseBad = false;
170  std::unique_lock<std::mutex> lock(_lcmMutex);
171  _waitingForLcmResponse = false;
172  _lcmCV.notify_all();
173  }
174 }
175 
177  _running = true;
178  this->start();
179  _lcmThread = std::thread(&RobotInterface::lcmHandler, this);
180  printf("[RobotInterface] Send parameters to robot...\n");
181  for (auto &kv : _controlParameters.collection._map) {
182  sendControlParameter(kv.first, kv.second->get(kv.second->_kind),
183  kv.second->_kind, false);
184  }
185 
186  for (auto &kv : _userParameters.collection._map) {
187  sendControlParameter(kv.first, kv.second->get(kv.second->_kind),
188  kv.second->_kind, true);
189  }
190 
191 }
192 
194  printf("stopInterface\n");
195  _running = false;
197  printf("stopall done\n");
198  _lcmThread.join();
199  printf("lcmthread joined\n");
200 }
201 
203  while (_running) {
204  _lcm.handleTimeout(1000);
205  }
206 }
RobotType _robotType
Vec3< T > bodyPosition
VisualizationData * _visualizationData
Definition: DrawList.h:51
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)
Definition: Graphics3D.cpp:122
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)
Definition: DrawList.h:170
void setState(const FBModelState< T > &state)
uint64_t _robotID
std::string getLcmUrl(s64 ttl)
Definition: utilities.cpp:32
size_t setupCheetah3(Vec4< float > color, bool useOld)
Definition: Graphics3D.cpp:117
FBModelState< double > _fwdKinState
std::string getConfigDirectoryPath()
Definition: utilities.cpp:30
#define INTERFACE_LCM_NAME
void handleControlParameter(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const control_parameter_respones_lcmt *msg)
uint32_t u32
Definition: cTypes.h:18
PeriodicTaskManager _taskManager
std::vector< QMatrix4x4 > _kinematicXform
Definition: DrawList.h:279
int8_t s8
Definition: cTypes.h:21
std::thread _lcmThread
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
Definition: cppTypes.h:30
#define TIMES_TO_RESEND_CONTROL_PARAM
control_parameter_request_lcmt _parameter_request_lcmt
Graphics3D * _gfx
void buildDrawList()
Definition: DrawList.cpp:266
GamepadCommand & getDriverCommand()
Definition: Graphics3D.h:52
void updateCheckerboard(T height, size_t id)
Definition: DrawList.h:229
lcm::LCM _lcm
Definition: RobotRunner.h:73
DynamicsSimulator< double > * _simulator
bool _pendingControlParameterSend
std::mutex _lcmMutex
bool _waitingForLcmResponse
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
Definition: cppTypes.h:102
Types to allow remote access of control parameters, for use with LCM/Shared memory.
RobotType
Definition: cppTypes.h:120
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)
Definition: DrawList.cpp:215
FloatingBaseModel< T > buildModel()
Definition: Quadruped.cpp:113
ControlParameters & _userParameters
DrawList _drawList
Definition: Graphics3D.h:49
RobotType robotType
Definition: RobotRunner.h:47