Commit d8a18254 authored by Rohan Budhiraja's avatar Rohan Budhiraja

[sot-hrp2-device]

* add setNoIntegration function for no integration of control
* move robotState to sot-core device
* map velocity sensor data, if available, to robotVelocity signal
parent f0c2869c
...@@ -76,6 +76,12 @@ setSecondOrderIntegration(void) ...@@ -76,6 +76,12 @@ setSecondOrderIntegration(void)
device_.setSecondOrderIntegration(); device_.setSecondOrderIntegration();
} }
void SoTHRP2Controller::
setNoIntegration(void)
{
device_.setNoIntegration();
}
void SoTHRP2Controller:: void SoTHRP2Controller::
nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn) nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{ {
......
...@@ -49,6 +49,7 @@ class SoTHRP2Controller: public ...@@ -49,6 +49,7 @@ class SoTHRP2Controller: public
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut); void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
void setSecondOrderIntegration(void); void setSecondOrderIntegration(void);
void setNoIntegration(void);
/// Embedded python interpreter accessible via Corba /// Embedded python interpreter accessible via Corba
boost::shared_ptr<dynamicgraph::Interpreter> interpreter_; boost::shared_ptr<dynamicgraph::Interpreter> interpreter_;
......
...@@ -34,7 +34,6 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName): ...@@ -34,7 +34,6 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
dgsot::Device(RobotName), dgsot::Device(RobotName),
timestep_(TIMESTEP_DEFAULT), timestep_(TIMESTEP_DEFAULT),
previousState_ (), previousState_ (),
robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState"),
accelerometerSOUT_ accelerometerSOUT_
("StackOfTasks(" + RobotName + ")::output(vector)::accelerometer"), ("StackOfTasks(" + RobotName + ")::output(vector)::accelerometer"),
gyrometerSOUT_ ("StackOfTasks(" + RobotName + ")::output(vector)::gyrometer"), gyrometerSOUT_ ("StackOfTasks(" + RobotName + ")::output(vector)::gyrometer"),
...@@ -50,7 +49,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName): ...@@ -50,7 +49,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
{ {
sotDEBUGIN(25) ; sotDEBUGIN(25) ;
for( int i=0;i<4;++i ) { withForceSignals[i] = true; } for( int i=0;i<4;++i ) { withForceSignals[i] = true; }
signalRegistration (robotState_ << accelerometerSOUT_ << gyrometerSOUT_ signalRegistration ( accelerometerSOUT_ << gyrometerSOUT_
<< currentSOUT_ << p_gainsSOUT_ << d_gainsSOUT_); << currentSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
dg::Vector data (3); data.setZero (); dg::Vector data (3); data.setZero ();
accelerometerSOUT_.setConstant (data); accelerometerSOUT_.setConstant (data);
...@@ -138,6 +137,36 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn) ...@@ -138,6 +137,36 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
robotState_.setTime(t); robotState_.setTime(t);
} }
it = SensorsIn.find("base_velocity");
bool ffVelSensorFound_ = false;
Eigen::VectorXd base_v(6);
if (it!=SensorsIn.end())
{
ffVelSensorFound_ = true;
const vector<double>& basevel = it->second.getValues ();
base_v << basevel[0], basevel[1], basevel[2],
basevel[3], basevel[4], basevel[5];
}
it = SensorsIn.find("joint_velocities");
if (it!=SensorsIn.end())
{
const vector<double>& velIn = it->second.getValues();
robotVelocity.resize (velIn.size () + 6);
if (ffVelSensorFound_ == true) {
robotVelocity.head<6>() = base_v;
}
else
robotVelocity.head<6>().setZero();
for (unsigned i = 0; i < velIn.size(); ++i)
robotVelocity (i + 6) = velIn[i];
robotVelocity_.setConstant(robotVelocity);
robotVelocity_.setTime(t);
}
it = SensorsIn.find("accelerometer_0"); it = SensorsIn.find("accelerometer_0");
if (it!=SensorsIn.end()) if (it!=SensorsIn.end())
{ {
...@@ -224,8 +253,7 @@ void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsI ...@@ -224,8 +253,7 @@ void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsI
void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut) void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
{ {
sotDEBUGIN(25) ; sotDEBUGIN(25) ;
vector<double> anglesOut; vector<double> controlOut_;
anglesOut.resize(state_.size());
// Integrate control // Integrate control
increment(timestep_); increment(timestep_);
...@@ -233,14 +261,11 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut) ...@@ -233,14 +261,11 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
sotDEBUG (25) << "diff = " << ((previousState_.size() == state_.size())? sotDEBUG (25) << "diff = " << ((previousState_.size() == state_.size())?
(state_ - previousState_) : state_ ) << std::endl; (state_ - previousState_) : state_ ) << std::endl;
previousState_ = state_; previousState_ = state_;
controlOut_.resize(state_.size()-6);
// Specify the joint values for the controller.
if (anglesOut.size()!=state_.size()-6)
anglesOut.resize(state_.size()-6);
for(unsigned int i=6; i < state_.size();++i) for(unsigned int i=6; i < state_.size();++i)
anglesOut[i-6] = state_(i); controlOut_[i-6] = state_(i);
controlOut["joints"].setValues(anglesOut); controlOut["joints"].setValues(controlOut_);
// Read zmp reference from input signal if plugged // Read zmp reference from input signal if plugged
int time = controlSIN.getTime (); int time = controlSIN.getTime ();
......
...@@ -63,14 +63,6 @@ protected: ...@@ -63,14 +63,6 @@ protected:
/// \brief Previous robot configuration. /// \brief Previous robot configuration.
dg::Vector previousState_; dg::Vector previousState_;
/// \brief Robot state provided by OpenHRP.
///
/// This corresponds to the real encoders values and take into
/// account the stabilization step. Therefore, this usually
/// does *not* match the state control input signal.
///
dynamicgraph::Signal<dg::Vector, int> robotState_;
/// Accelerations measured by accelerometers /// Accelerations measured by accelerometers
dynamicgraph::Signal <dg::Vector, int> accelerometerSOUT_; dynamicgraph::Signal <dg::Vector, int> accelerometerSOUT_;
/// Rotation velocity measured by gyrometers /// Rotation velocity measured by gyrometers
...@@ -85,6 +77,7 @@ protected: ...@@ -85,6 +77,7 @@ protected:
/// Intermediate variables to avoid allocation during control /// Intermediate variables to avoid allocation during control
dg::Vector mlforces; dg::Vector mlforces;
dg::Vector mlRobotState; dg::Vector mlRobotState;
dg::Vector robotVelocity;
dgsot::MatrixRotation pose; dgsot::MatrixRotation pose;
dg::Vector accelerometer_; dg::Vector accelerometer_;
dg::Vector gyrometer_; dg::Vector gyrometer_;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment