Commit 860a2491 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Optimize code to avoid dynamic allocation in control loop.

parent f3a58f9d
......@@ -34,7 +34,8 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
dgsot::Device(RobotName),
timestep_(TIMESTEP_DEFAULT),
previousState_ (),
robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState")
robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState"),
mlforces (6)
{
sotDEBUGIN(25) ;
signalRegistration (robotState_);
......@@ -75,7 +76,6 @@ void SoTHRP2Device::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
stateSOUT.setConstant (state);
// Implements force recollection.
ml::Vector mlforces(6);
vector<double> forcesIn = SensorsIn["forces"].getValues();
for(int i=0;i<4;++i)
{
......@@ -98,7 +98,6 @@ void SoTHRP2Device::nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn
// Read force values.
vector<double> forcesIn = SensorsIn["forces"].getValues();
ml::Vector mlforces (6);
int t = controlSIN.getTime();
for(int i = 0; i < 4; ++i)
{
......@@ -117,7 +116,6 @@ void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsI
updateRobotState(anglesIn);
ml::Vector mlforces (6);
for(int i = 0; i < 4; ++i)
{
for (int j = 0; j < 6; ++j)
......
......@@ -70,6 +70,9 @@ protected:
///
dynamicgraph::Signal<ml::Vector, int> robotState_;
/// Intermediate variables to avoid allocation during control
ml::Vector mlforces;
ml::Vector mlRobotState;
};
#endif /* _SOT_HRP2Device_H_*/
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