#include <RobotInterface.h>

Inheritance diagram for RobotInterface:

Collaboration diagram for RobotInterface:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	RobotInterface (RobotType robotType, Graphics3D gfx, PeriodicTaskManager tm, ControlParameters &userParameters)

RobotControlParameters &	getParams ()

void	startInterface ()

void	stopInterface ()

void	lcmHandler ()

void	sendControlParameter (const std::string &name, ControlParameterValue value, ControlParameterValueKind kind, bool isUser)

void	handleControlParameter (const lcm::ReceiveBuffer rbuf, const std::string &chan, const control_parameter_respones_lcmt msg)

void	handleVisualizationData (const lcm::ReceiveBuffer rbuf, const std::string &chan, const cheetah_visualization_lcmt msg)

void	init ()

void	run ()

void	cleanup ()

virtual	~RobotInterface ()

Private Attributes
PeriodicTaskManager	_taskManager

gamepad_lcmt	_gamepad_lcmt

control_parameter_request_lcmt	_parameter_request_lcmt

bool	_pendingControlParameterSend = false

lcm::LCM	_lcm

uint64_t	_robotID

std::thread	_lcmThread

VisualizationData	_visualizationData

RobotControlParameters	_controlParameters

ControlParameters &	_userParameters

Graphics3D *	_gfx

RobotType	_robotType

bool	_running = false

std::mutex	_lcmMutex

std::condition_variable	_lcmCV

bool	_waitingForLcmResponse = false

bool	_lcmResponseBad = true

Quadruped< double >	_quadruped

FloatingBaseModel< double >	_model

DynamicsSimulator< double > *	_simulator = nullptr

FBModelState< double >	_fwdKinState

Additional Inherited Members
Private Member Functions inherited from PeriodicTask
	PeriodicTask (PeriodicTaskManager *taskManager, float period, std::string name)

void	start ()

void	stop ()

void	printStatus ()

void	clearMax ()

bool	isSlow ()

virtual	~PeriodicTask ()

float	getPeriod ()

float	getRuntime ()

float	getMaxPeriod ()

float	getMaxRuntime ()

Detailed Description

Definition at line 26 of file RobotInterface.h.

Constructor & Destructor Documentation

RobotInterface::RobotInterface	(	RobotType	robotType,
		Graphics3D *	gfx,
		PeriodicTaskManager *	tm,
		ControlParameters &	userParameters
	)

Definition at line 8 of file RobotInterface.cpp.

References _controlParameters, Graphics3D::_drawList, _fwdKinState, _gfx, DrawList::_kinematicXform, _lcm, _model, _parameter_request_lcmt, _quadruped, _robotID, _robotType, _simulator, DrawList::_visualizationData, _visualizationData, DrawList::addCheckerboard(), DrawList::buildDrawList(), Quadruped< T >::buildModel(), CHEETAH_3, CHEETAH_3_DEFAULT_PARAMETERS, ControlParameters::generateUnitializedList(), getConfigDirectoryPath(), handleControlParameter(), handleVisualizationData(), ControlParameters::initializeFromYamlFile(), ControlParameters::isFullyInitialized(), MINI_CHEETAH, MINI_CHEETAH_DEFAULT_PARAMETERS, FBModelState< T >::q, FBModelState< T >::qd, RobotRunner::robotType, Graphics3D::setupCheetah3(), Graphics3D::setupMiniCheetah(), and DrawList::updateCheckerboard().

     : PeriodicTask(tm, ROBOT_INTERFACE_UPDATE_PERIOD, "robot-interface"),
       _lcm(getLcmUrl(255)),
       _userParameters(userParameters) {
   _parameter_request_lcmt.requestNumber = 0;
   _gfx = gfx;
   _robotType = robotType;
   printf("[RobotInterface] Load parameters...\n");
   if (_robotType == RobotType::MINI_CHEETAH) {
     _controlParameters.initializeFromYamlFile(getConfigDirectoryPath() +
                                               MINI_CHEETAH_DEFAULT_PARAMETERS);
   } else if (_robotType == RobotType::CHEETAH_3) {
     _controlParameters.initializeFromYamlFile(getConfigDirectoryPath() +
                                               CHEETAH_3_DEFAULT_PARAMETERS);
   } else {
     assert(false);
   }
 
   if (!_controlParameters.isFullyInitialized()) {
     printf("Not all robot control parameters were initialized. Missing:\n%s\n",
            _controlParameters.generateUnitializedList().c_str());
     throw std::runtime_error("not all parameters initialized from ini file");
   }
   printf("[RobotInterface] Init LCM\n");
   printf("[RobotInterface] Init graphics\n");
   Vec4<float> robotColor;
   robotColor << 0.2, 0.2, 0.6, 0.6;
   _robotID = _robotType == RobotType::MINI_CHEETAH ? gfx->setupMiniCheetah(robotColor, true)
                                                    : gfx->setupCheetah3(robotColor, true);
   printf("draw list has %lu items\n", _gfx->_drawList._kinematicXform.size());
   _gfx->_drawList._visualizationData = &_visualizationData;
   Checkerboard checker(10, 10, 10, 10);
   uint64_t floorID = _gfx->_drawList.addCheckerboard(checker);
   _gfx->_drawList.updateCheckerboard(0, floorID);
   _gfx->_drawList.buildDrawList();
 
   _lcm.subscribe("interface_response", &RobotInterface::handleControlParameter,
                  this);
   _lcm.subscribe("main_cheetah_visualization",
                  &RobotInterface::handleVisualizationData, this);
 
   printf("[RobotInterface] Init dynamics\n");
   _quadruped = robotType == RobotType::MINI_CHEETAH ? buildMiniCheetah<double>()
                                                     : buildCheetah3<double>();
   _model = _quadruped.buildModel();
   _simulator = new DynamicsSimulator<double>(_model, false);
   DVec<double> zero12(12);
   for (u32 i = 0; i < 12; i++) {
     zero12[i] = 0.;
   }
 
   _fwdKinState.q = zero12;
   _fwdKinState.qd = zero12;
 }

Here is the call graph for this function:

virtual RobotInterface::~RobotInterface ( )

inlinevirtual

Definition at line 50 of file RobotInterface.h.

References _simulator, and PeriodicTask::stop().

                             {
     delete _simulator;
     stop();
   }

Here is the call graph for this function:

Member Function Documentation

void RobotInterface::cleanup ( )

inlinevirtual

Implements PeriodicTask.

Definition at line 49 of file RobotInterface.h.

49 {}

RobotControlParameters& RobotInterface::getParams ( )

inline

Definition at line 31 of file RobotInterface.h.

References _controlParameters, handleControlParameter(), handleVisualizationData(), lcmHandler(), sendControlParameter(), startInterface(), and stopInterface().

31 { return _controlParameters; }

RobotInterface::_controlParameters

RobotControlParameters _controlParameters

Definition: RobotInterface.h:64

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotInterface::handleControlParameter	(	const lcm::ReceiveBuffer *	rbuf,
		const std::string &	chan,
		const control_parameter_respones_lcmt *	msg
	)

Definition at line 152 of file RobotInterface.cpp.

References _lcmCV, _lcmMutex, _lcmResponseBad, _parameter_request_lcmt, and _waitingForLcmResponse.

                                                 {
   (void)rbuf;
   (void)chan;
   if (!_waitingForLcmResponse) {
     printf(
         "[RobotInterface] Got a control parameter response when we weren't "
         "expecting one, ignoring it!\n");
     _lcmResponseBad = true;
     return;
   }
 
   // got a real response, let's check it
   if (msg->requestNumber == _parameter_request_lcmt.requestNumber &&
       msg->parameterKind == _parameter_request_lcmt.parameterKind &&
       std::string((char *)msg->name) == (char *)_parameter_request_lcmt.name) {
     _lcmResponseBad = false;
     std::unique_lock<std::mutex> lock(_lcmMutex);
     _waitingForLcmResponse = false;
     _lcmCV.notify_all();
   }
 }

Here is the caller graph for this function:

void RobotInterface::handleVisualizationData	(	const lcm::ReceiveBuffer *	rbuf,
		const std::string &	chan,
		const cheetah_visualization_lcmt *	msg
	)

Definition at line 64 of file RobotInterface.cpp.

References _fwdKinState, _simulator, FBModelState< T >::bodyOrientation, FBModelState< T >::bodyPosition, DynamicsSimulator< T >::forwardKinematics(), FBModelState< T >::q, and DynamicsSimulator< T >::setState().

                                            {
   (void)rbuf;
   (void)chan;
   for (int i = 0; i < 3; i++) {
     _fwdKinState.bodyPosition[i] = msg->x[i];
   }
 
   for (int i = 0; i < 4; i++) {
     _fwdKinState.bodyOrientation[i] = msg->quat[i];
   }
 
   for (int i = 0; i < 12; i++) {
     _fwdKinState.q[i] = msg->q[i];
   }
 
   _simulator->setState(_fwdKinState);
   _simulator->forwardKinematics();
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotInterface::init ( )

inlinevirtual

Implements PeriodicTask.

Definition at line 47 of file RobotInterface.h.

References run().

47 {}

Here is the call graph for this function:

void RobotInterface::lcmHandler ( )

Definition at line 202 of file RobotInterface.cpp.

References _lcm, and _running.

                                 {
   while (_running) {
     _lcm.handleTimeout(1000);
   }
 }

Here is the caller graph for this function:

void RobotInterface::run ( )

virtual

Implements PeriodicTask.

Definition at line 85 of file RobotInterface.cpp.

References Graphics3D::_drawList, _gamepad_lcmt, _gfx, _lcm, _robotID, _simulator, GamepadCommand::get(), Graphics3D::getDriverCommand(), INTERFACE_LCM_NAME, and DrawList::updateRobotFromModel().

                          {
   if (_gfx) {
     _gfx->_drawList.updateRobotFromModel(*_simulator, _robotID, true);
     _gfx->update();
     _gfx->getDriverCommand().get(&_gamepad_lcmt);
     _lcm.publish(INTERFACE_LCM_NAME, &_gamepad_lcmt);
   }
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotInterface::sendControlParameter	(	const std::string &	name,
		ControlParameterValue	value,
		ControlParameterValueKind	kind,
		bool	isUser
	)

Definition at line 96 of file RobotInterface.cpp.

References _lcm, _lcmCV, _lcmMutex, _lcmResponseBad, _parameter_request_lcmt, _pendingControlParameterSend, _waitingForLcmResponse, controlParameterValueToString(), SET_ROBOT_PARAM_BY_NAME, SET_USER_PARAM_BY_NAME, and TIMES_TO_RESEND_CONTROL_PARAM.

                                                                                        {
   if (_pendingControlParameterSend) {
     printf(
         "[ERROR] trying to send control parameter while a send is in progress, "
         "ignoring!\n");
     return;
   }
   _pendingControlParameterSend = true;
   for (int iteration = 0; iteration < TIMES_TO_RESEND_CONTROL_PARAM;) {
     // new message
     _parameter_request_lcmt.requestNumber++;
 
     // message data
     _parameter_request_lcmt.requestKind = isUser ?
         (s8)ControlParameterRequestKind::SET_USER_PARAM_BY_NAME : (s8)ControlParameterRequestKind::SET_ROBOT_PARAM_BY_NAME;
     strcpy((char *)_parameter_request_lcmt.name, name.c_str());
     memcpy(_parameter_request_lcmt.value, &value, sizeof(value));
     _parameter_request_lcmt.parameterKind = (s8)kind;
     printf("set %s to %s (%d)\n", name.c_str(),
            controlParameterValueToString(value, kind).c_str(), iteration);
 
     // send
     _lcm.publish("interface_request", &_parameter_request_lcmt);
 
     // wait for response with timeout
     _waitingForLcmResponse = true;
     _lcmResponseBad = true;
     std::unique_lock<std::mutex> lock(_lcmMutex);
 
     if (_lcmCV.wait_for(lock, 100ms) == std::cv_status::no_timeout) {
       _waitingForLcmResponse = false;
       // check it
       if (_waitingForLcmResponse || _lcmResponseBad) {
         printf(
             "[RobotInterface] Failed to send parameter %s (iter %d) "
             "wakeup %d bad? %d\n",
             name.c_str(), iteration, _waitingForLcmResponse, _lcmResponseBad);
         usleep(100000);  // sleep a bit to let other bad sends happen
       } else {
         iteration++;
       }
     } else {
       _waitingForLcmResponse = false;
       // fail!
       printf(
           "[RobotInterface] Failed to send parameter %s (iter %d timed out), "
           "trying again...\n",
           name.c_str(), iteration);
       usleep(100000);  // sleep a bit to let other bad sends happen
     }
   }
   _pendingControlParameterSend = false;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotInterface::startInterface ( )

Definition at line 176 of file RobotInterface.cpp.

References _controlParameters, _lcmThread, ControlParameterCollection::_map, _running, _userParameters, ControlParameters::collection, lcmHandler(), sendControlParameter(), and PeriodicTask::start().

                                     {
   _running = true;
   this->start();
   _lcmThread = std::thread(&RobotInterface::lcmHandler, this);
   printf("[RobotInterface] Send parameters to robot...\n");
   for (auto &kv : _controlParameters.collection._map) {
     sendControlParameter(kv.first, kv.second->get(kv.second->_kind),
                          kv.second->_kind, false);
   }
 
   for (auto &kv : _userParameters.collection._map) {
     sendControlParameter(kv.first, kv.second->get(kv.second->_kind),
                          kv.second->_kind, true);
   }
   
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotInterface::stopInterface ( )

Definition at line 193 of file RobotInterface.cpp.

References _lcmThread, _running, _taskManager, and PeriodicTaskManager::stopAll().

                                    {
   printf("stopInterface\n");
   _running = false;
   _taskManager.stopAll();
   printf("stopall done\n");
   _lcmThread.join();
   printf("lcmthread joined\n");
 }

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

RobotControlParameters RobotInterface::_controlParameters

private

Definition at line 64 of file RobotInterface.h.

FBModelState<double> RobotInterface::_fwdKinState

private

Definition at line 79 of file RobotInterface.h.

gamepad_lcmt RobotInterface::_gamepad_lcmt

private

Definition at line 57 of file RobotInterface.h.

Graphics3D* RobotInterface::_gfx

private

Definition at line 66 of file RobotInterface.h.

lcm::LCM RobotInterface::_lcm

private

Definition at line 60 of file RobotInterface.h.

std::condition_variable RobotInterface::_lcmCV

private

Definition at line 71 of file RobotInterface.h.

std::mutex RobotInterface::_lcmMutex

private

Definition at line 70 of file RobotInterface.h.

bool RobotInterface::_lcmResponseBad = true

private

Definition at line 73 of file RobotInterface.h.

std::thread RobotInterface::_lcmThread

private

Definition at line 62 of file RobotInterface.h.

FloatingBaseModel<double> RobotInterface::_model

private

Definition at line 77 of file RobotInterface.h.

control_parameter_request_lcmt RobotInterface::_parameter_request_lcmt

private

Definition at line 58 of file RobotInterface.h.

bool RobotInterface::_pendingControlParameterSend = false

private

Definition at line 59 of file RobotInterface.h.

Quadruped<double> RobotInterface::_quadruped

private

Definition at line 76 of file RobotInterface.h.

uint64_t RobotInterface::_robotID

private

Definition at line 61 of file RobotInterface.h.

RobotType RobotInterface::_robotType

private

Definition at line 67 of file RobotInterface.h.

bool RobotInterface::_running = false

private

Definition at line 68 of file RobotInterface.h.

DynamicsSimulator<double>* RobotInterface::_simulator = nullptr

private

Definition at line 78 of file RobotInterface.h.

PeriodicTaskManager RobotInterface::_taskManager

private

Definition at line 56 of file RobotInterface.h.

ControlParameters& RobotInterface::_userParameters

private

Definition at line 65 of file RobotInterface.h.

VisualizationData RobotInterface::_visualizationData

private

Definition at line 63 of file RobotInterface.h.

bool RobotInterface::_waitingForLcmResponse = false

private

Definition at line 72 of file RobotInterface.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation