From f5745224a859e4f9f7d890ad39bd4254d39c5084 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Thu, 6 Sep 2012 12:02:39 +0200
Subject: [PATCH] Optimize code to avoid dynamic allocation in control loop.
---
include/sot/core/device.hh | 3 +++
src/tools/device.cpp | 37 ++++++++++++++++---------------------
2 files changed, 19 insertions(+), 21 deletions(-)
diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh
index bc485f00..187f4a21 100644
--- a/include/sot/core/device.hh
+++ b/include/sot/core/device.hh
@@ -117,6 +117,9 @@ namespace dynamicgraph {
public:
void setRoot( const ml::Matrix & root );
void setRoot( const MatrixHomogeneous & worldMwaist );
+ private:
+ // Intermediate variable to avoid dynamic allocation
+ ml::Vector forceZero6;
};
} // namespace sot
} // namespace dynamicgraph
diff --git a/src/tools/device.cpp b/src/tools/device.cpp
index 4c1b9e79..a60bd1bb 100644
--- a/src/tools/device.cpp
+++ b/src/tools/device.cpp
@@ -55,26 +55,20 @@ void Device::integrateRollPitchYaw(ml::Vector& state, const ml::Vector& control,
double roll = state(3);
double pitch = state(4);
double yaw = state(5);
- std::vector<jrlMathTools::Vector3D<double> > column;
+ jrlMathTools::Vector3D<double> column [3];
// Build rotation matrix as a vector of colums
- jrlMathTools::Vector3D<double> e1;
- e1(0) = cos(pitch)*cos(yaw);
- e1(1) = cos(pitch)*sin(yaw);
- e1(2) = -sin(pitch);
- column.push_back(e1);
-
- jrlMathTools::Vector3D<double> e2;
- e2(0) = sin(roll)*sin(pitch)*cos(yaw) - cos(roll)*sin(yaw);
- e2(1) = sin(roll)*sin(pitch)*sin(yaw) + cos(roll)*cos(yaw);
- e2(2) = sin(roll)*cos(pitch);
- column.push_back(e2);
-
- jrlMathTools::Vector3D<double> e3;
- e3(0) = cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw);
- e3(1) = cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw);
- e3(2) = cos(roll)*cos(pitch);
- column.push_back(e3);
+ column [0](0) = cos(pitch)*cos(yaw);
+ column [0](1) = cos(pitch)*sin(yaw);
+ column [0](2) = -sin(pitch);
+
+ column [1](0) = sin(roll)*sin(pitch)*cos(yaw) - cos(roll)*sin(yaw);
+ column [1](1) = sin(roll)*sin(pitch)*sin(yaw) + cos(roll)*cos(yaw);
+ column [1](2) = sin(roll)*cos(pitch);
+
+ column [2](0) = cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw);
+ column [2](1) = cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw);
+ column [2](2) = cos(roll)*cos(pitch);
// Apply Rodrigues (1795–1851) formula for rotation about omega vector
double angle = dt*omega.norm();
@@ -129,8 +123,10 @@ Device( const std::string& n )
,pseudoTorqueSOUT( "Device::output(vector)::ptorque" )
,previousControlSOUT( "Device("+n+")::output(vector)::previousControl" )
,motorcontrolSOUT( "Device("+n+")::output(vector)::motorcontrol" )
- ,ZMPPreviousControllerSOUT( "Device("+n+")::output(vector)::zmppreviouscontroller" ), ffPose_()
+ ,ZMPPreviousControllerSOUT( "Device("+n+")::output(vector)::zmppreviouscontroller" ), ffPose_(),
+ forceZero6 (6)
{
+ forceZero6.fill (0);
/* --- SIGNALS --- */
for( int i=0;i<4;++i ){ withForceSignals[i] = false; }
forcesSOUT[0] =
@@ -227,9 +223,8 @@ increment( const double & dt )
/* Position the signals corresponding to sensors. */
stateSOUT .setConstant( state_ ); stateSOUT.setTime( time+1 );
- ml::Vector forceNull(6); forceNull.fill(0);
for( int i=0;i<4;++i ){
- if( !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceNull);
+ if( !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceZero6);
}
ml::Vector zmp(3); zmp.fill( .0 );
ZMPPreviousControllerSOUT .setConstant( zmp );
--
GitLab