Commit ab6263d4 authored by Olivier Stasse's avatar Olivier Stasse

Merge branch 'master' of github.com:stack-of-tasks/sot-hrp2 into topic/olivier

parents c44ef33a 19496ce2
......@@ -76,6 +76,12 @@ setSecondOrderIntegration(void)
device_.setSecondOrderIntegration();
}
void SoTHRP2Controller::
setNoIntegration(void)
{
device_.setNoIntegration();
}
void SoTHRP2Controller::
nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
......
......@@ -49,6 +49,7 @@ class SoTHRP2Controller: public
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
void setSecondOrderIntegration(void);
void setNoIntegration(void);
/// Embedded python interpreter accessible via Corba
boost::shared_ptr<dynamicgraph::Interpreter> interpreter_;
......
......@@ -34,7 +34,6 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
dgsot::Device(RobotName),
timestep_(TIMESTEP_DEFAULT),
previousState_ (),
robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState"),
accelerometerSOUT_
("StackOfTasks(" + RobotName + ")::output(vector)::accelerometer"),
gyrometerSOUT_ ("StackOfTasks(" + RobotName + ")::output(vector)::gyrometer"),
......@@ -50,7 +49,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
{
sotDEBUGIN(25) ;
for( int i=0;i<4;++i ) { withForceSignals[i] = true; }
signalRegistration (robotState_ << accelerometerSOUT_ << gyrometerSOUT_
signalRegistration ( accelerometerSOUT_ << gyrometerSOUT_
<< currentSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
dg::Vector data (3); data.setZero ();
accelerometerSOUT_.setConstant (data);
......@@ -98,8 +97,10 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
}
it = SensorsIn.find("attitude");
bool attitudeSensorFound_ = false;
if (it!=SensorsIn.end())
{
attitudeSensorFound_ = true;
const vector<double>& attitude = it->second.getValues ();
for (unsigned int i = 0; i < 3; ++i)
for (unsigned int j = 0; j < 3; ++j)
......@@ -108,19 +109,64 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
attitudeSOUT.setTime (t);
}
it = SensorsIn.find("base_position");
bool ffPositionSensorFound_ = false;
Eigen::Vector3d base_p;
if (it!=SensorsIn.end())
{
ffPositionSensorFound_ = true;
const vector<double>& baseposition = it->second.getValues ();
base_p << baseposition[0], baseposition[1], baseposition[2];
}
it = SensorsIn.find("joints");
if (it!=SensorsIn.end())
{
const vector<double>& anglesIn = it->second.getValues();
mlRobotState.resize (anglesIn.size () + 6);
for (unsigned i = 0; i < 6; ++i)
mlRobotState (i) = 0.;
if (ffPositionSensorFound_ == true && attitudeSensorFound_ == true) {
mlRobotState.head<3>() = base_p;
mlRobotState.segment<3>(3) = pose.eulerAngles(2,1,0).reverse();
}
else
mlRobotState.head<6>().setZero();
for (unsigned i = 0; i < anglesIn.size(); ++i)
mlRobotState (i + 6) = anglesIn[i];
robotState_.setConstant(mlRobotState);
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");
if (it!=SensorsIn.end())
{
......@@ -207,8 +253,7 @@ void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsI
void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
{
sotDEBUGIN(25) ;
vector<double> anglesOut;
anglesOut.resize(state_.size());
vector<double> controlOut_;
// Integrate control
increment(timestep_);
......@@ -216,14 +261,11 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
sotDEBUG (25) << "diff = " << ((previousState_.size() == state_.size())?
(state_ - previousState_) : state_ ) << std::endl;
previousState_ = state_;
// Specify the joint values for the controller.
if (anglesOut.size()!=state_.size()-6)
anglesOut.resize(state_.size()-6);
controlOut_.resize(state_.size()-6);
for(unsigned int i=6; i < state_.size();++i)
anglesOut[i-6] = state_(i);
controlOut["joints"].setValues(anglesOut);
controlOut_[i-6] = state_(i);
controlOut["control"].setValues(controlOut_);
// Read zmp reference from input signal if plugged
int time = controlSIN.getTime ();
......
......@@ -63,14 +63,6 @@ protected:
/// \brief Previous robot configuration.
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
dynamicgraph::Signal <dg::Vector, int> accelerometerSOUT_;
/// Rotation velocity measured by gyrometers
......@@ -85,6 +77,7 @@ protected:
/// Intermediate variables to avoid allocation during control
dg::Vector mlforces;
dg::Vector mlRobotState;
dg::Vector robotVelocity;
dgsot::MatrixRotation pose;
dg::Vector accelerometer_;
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