Commit 19496ce2 authored by Rohan Budhiraja's avatar Rohan Budhiraja Committed by GitHub

Merge pull request #15 from proyan/master

[sot-hrp2-device]
parents f0c2869c 099af79b
......@@ -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);
......@@ -138,6 +137,36 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
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())
{
......@@ -224,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_);
......@@ -233,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