Skip to content
Snippets Groups Projects
Commit 1cbd53b4 authored by andreadelprete's avatar andreadelprete
Browse files

Fix the formatting using GNU code style

parent c7f33d17
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/sot/core/device.hh
+ 10
− 10
View file @ 1cbd53b4
......@@ -43,20 +43,20 @@ namespace dynamicgraph {
/* --------------------------------------------------------------------- */
class SOT_CORE_EXPORT Device
:public Entity
:public Entity
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName(void) const {
return CLASS_NAME;
return CLASS_NAME;
}
enum ForceSignalSource
{
FORCE_SIGNAL_RLEG,
FORCE_SIGNAL_LLEG,
FORCE_SIGNAL_RARM,
FORCE_SIGNAL_LARM
FORCE_SIGNAL_RLEG,
FORCE_SIGNAL_LLEG,
FORCE_SIGNAL_RARM,
FORCE_SIGNAL_LARM
};
protected:
......@@ -85,8 +85,8 @@ namespace dynamicgraph {
public: /* --- DISPLAY --- */
virtual void display(std::ostream& os) const;
SOT_CORE_EXPORT friend std::ostream&
operator<<(std::ostream& os,const Device& r) {
r.display(os); return os;
operator<<(std::ostream& os,const Device& r) {
r.display(os); return os;
}
public: /* --- SIGNALS --- */
......@@ -110,11 +110,11 @@ namespace dynamicgraph {
public: /* --- COMMANDS --- */
void commandLine(const std::string&, std::istringstream&,
std::ostream&){}
std::ostream&){}
protected:
/// Compute roll pitch yaw angles of freeflyer joint.
void integrateRollPitchYaw(ml::Vector& state, const ml::Vector& control,
double dt);
double dt);
/// Store Position of free flyer joint
MatrixHomogeneous ffPose_;
/// Compute the new position, from the current control.
......
src/tools/device.cpp
+ 97
− 97
View file @ 1cbd53b4
......@@ -42,15 +42,15 @@ const std::string Device::CLASS_NAME = "Device";
/* --------------------------------------------------------------------- */
void Device::integrateRollPitchYaw(ml::Vector& state, const ml::Vector& control,
double dt)
double dt)
{
jrlMathTools::Vector3D<double> omega;
// Translation part
for (unsigned int i=0; i<3; i++) {
state(i) += control(i)*dt;
ffPose_(i,3) = state(i);
omega(i) = control(i+3);
}
state(i) += control(i)*dt;
ffPose_(i,3) = state(i);
omega(i) = control(i+3);
}
// Rotation part
double roll = state(3);
double pitch = state(4);
......@@ -73,26 +73,26 @@ void Device::integrateRollPitchYaw(ml::Vector& state, const ml::Vector& control,
// Apply Rodrigues (1795–1851) formula for rotation about omega vector
double angle = dt*omega.norm();
if (angle == 0) {
return;
}
return;
}
jrlMathTools::Vector3D<double> k = omega/omega.norm();
// ei <- ei cos(angle) + sin(angle)(k ^ ei) + (k.ei)(1-cos(angle))k
for (unsigned int i=0; i<3; i++) {
jrlMathTools::Vector3D<double> ei = column[i];
column[i] = ei*cos(angle) + (k^ei)*sin(angle) + k*((k*ei)*(1-cos(angle)));
}
jrlMathTools::Vector3D<double> ei = column[i];
column[i] = ei*cos(angle) + (k^ei)*sin(angle) + k*((k*ei)*(1-cos(angle)));
}
// Store new position if ffPose_ member.
for (unsigned int r = 0; r < 3; r++) {
for (unsigned int c = 0; c < 3; c++) {
ffPose_(r,c) = column[c](r);
for (unsigned int c = 0; c < 3; c++) {
ffPose_(r,c) = column[c](r);
}
}
}
const double & nx = column[2](2);
const double & ny = column[1](2);
state(3) = atan2(ny,nx);
state(4) = atan2(-column[0](2),
sqrt(ny*ny+nx*nx));
sqrt(ny*ny+nx*nx));
state(5) = atan2(column[0](1),column[0](0));
}
......@@ -106,8 +106,8 @@ Device::
~Device( )
{
for( unsigned int i=0; i<4; ++i ) {
delete forcesSOUT[i];
}
delete forcesSOUT[i];
}
}
Device::
......@@ -117,7 +117,7 @@ Device( const std::string& n )
,vel_controlInit_(false)
,secondOrderIntegration_(false)
,controlSIN( NULL,"Device("+n+")::input(double)::control" )
//,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
//,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
,attitudeSIN(NULL,"Device::input(vector3)::attitudeIN")
,zmpSIN(NULL,"Device::input(vector3)::zmp")
,stateSOUT( "Device("+n+")::output(vector)::state" )
......@@ -127,24 +127,24 @@ Device( const std::string& n )
,previousControlSOUT( "Device("+n+")::output(vector)::previousControl" )
,motorcontrolSOUT( "Device("+n+")::output(vector)::motorcontrol" )
,ZMPPreviousControllerSOUT( "Device("+n+")::output(vector)::zmppreviouscontroller" ), ffPose_(),
forceZero6 (6)
forceZero6 (6)
{
forceZero6.fill (0);
/* --- SIGNALS --- */
for( int i=0;i<4;++i ){ withForceSignals[i] = false; }
forcesSOUT[0] =
new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceRLEG");
new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceRLEG");
forcesSOUT[1] =
new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLLEG");
new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLLEG");
forcesSOUT[2] =
new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceRARM");
new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceRARM");
forcesSOUT[3] =
new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLARM");
new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLARM");
signalRegistration( controlSIN<<stateSOUT<<velocitySOUT<<attitudeSOUT
<<attitudeSIN<<zmpSIN <<*forcesSOUT[0]<<*forcesSOUT[1]
<<*forcesSOUT[2]<<*forcesSOUT[3] <<previousControlSOUT
<<pseudoTorqueSOUT << motorcontrolSOUT << ZMPPreviousControllerSOUT );
<<attitudeSIN<<zmpSIN <<*forcesSOUT[0]<<*forcesSOUT[1]
<<*forcesSOUT[2]<<*forcesSOUT[3] <<previousControlSOUT
<<pseudoTorqueSOUT << motorcontrolSOUT << ZMPPreviousControllerSOUT );
state_.fill(.0); stateSOUT.setConstant( state_ );
velocity_.resize(state_.size()); velocity_.setZero();
......@@ -155,46 +155,46 @@ Device( const std::string& n )
std::string docstring;
/* Command setStateSize. */
docstring =
"\n"
" Set size of state vector\n"
"\n";
"\n"
" Set size of state vector\n"
"\n";
addCommand("resize",
new command::Setter<Device, unsigned int>
(*this, &Device::setStateSize, docstring));
new command::Setter<Device, unsigned int>
(*this, &Device::setStateSize, docstring));
docstring =
"\n"
" Set state vector value\n"
"\n";
"\n"
" Set state vector value\n"
"\n";
addCommand("set",
new command::Setter<Device, Vector>
(*this, &Device::setState, docstring));
new command::Setter<Device, Vector>
(*this, &Device::setState, docstring));
docstring =
"\n"
" Set velocity vector value\n"
"\n";
"\n"
" Set velocity vector value\n"
"\n";
addCommand("setVelocity",
new command::Setter<Device, Vector>
(*this, &Device::setVelocity, docstring));
new command::Setter<Device, Vector>
(*this, &Device::setVelocity, docstring));
void(Device::*setRootPtr)(const ml::Matrix&) = &Device::setRoot;
docstring
= command::docCommandVoid1("Set the root position.",
"matrix homogeneous");
= command::docCommandVoid1("Set the root position.",
"matrix homogeneous");
addCommand("setRoot",
command::makeCommandVoid1(*this,setRootPtr,
docstring));
command::makeCommandVoid1(*this,setRootPtr,
docstring));
/* Second Order Integration set. */
docstring =
"\n"
" Set the position calculous starting from \n"
" acceleration measure instead of velocity \n"
"\n";
"\n"
" Set the position calculous starting from \n"
" acceleration measure instead of velocity \n"
"\n";
addCommand("setSecondOrderIntegration",
command::makeCommandVoid0(*this,&Device::setSecondOrderIntegration,
docstring));
command::makeCommandVoid0(*this,&Device::setSecondOrderIntegration,
docstring));
// Handle commands and signals called in a synchronous way.
periodicCallBefore_.addSpecificCommands(*this, commandMap, "before.");
......@@ -275,27 +275,27 @@ increment( const double & dt )
// Run Synchronous commands and evaluate signals outside the main
// connected component of the graph.
try
{
periodicCallBefore_.run(time+1);
}
{
periodicCallBefore_.run(time+1);
}
catch (std::exception& e)
{
std::cerr
<< "exception caught while running periodical commands (before): "
<< e.what () << std::endl;
}
{
std::cerr
<< "exception caught while running periodical commands (before): "
<< e.what () << std::endl;
}
catch (const char* str)
{
std::cerr
<< "exception caught while running periodical commands (before): "
<< str << std::endl;
}
{
std::cerr
<< "exception caught while running periodical commands (before): "
<< str << std::endl;
}
catch (...)
{
std::cerr
<< "unknown exception caught while"
<< " running periodical commands (before)" << std::endl;
}
{
std::cerr
<< "unknown exception caught while"
<< " running periodical commands (before)" << std::endl;
}
/* Force the recomputation of the control. */
......@@ -308,7 +308,7 @@ increment( const double & dt )
/* Position the signals corresponding to sensors. */
stateSOUT .setConstant( state_ ); stateSOUT.setTime( time+1 );
//computation of the velocity signal
//computation of the velocity signal
if( secondOrderIntegration_ )
{
velocitySOUT.setConstant( velocity_ );
......@@ -320,35 +320,35 @@ increment( const double & dt )
velocitySOUT.setTime( time+1 );
}
for( int i=0;i<4;++i ){
if( !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceZero6);
}
if( !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceZero6);
}
ml::Vector zmp(3); zmp.fill( .0 );
ZMPPreviousControllerSOUT .setConstant( zmp );
// Run Synchronous commands and evaluate signals outside the main
// connected component of the graph.
try
{
periodicCallAfter_.run(time+1);
}
{
periodicCallAfter_.run(time+1);
}
catch (std::exception& e)
{
std::cerr
<< "exception caught while running periodical commands (after): "
<< e.what () << std::endl;
}
{
std::cerr
<< "exception caught while running periodical commands (after): "
<< e.what () << std::endl;
}
catch (const char* str)
{
std::cerr
<< "exception caught while running periodical commands (after): "
<< str << std::endl;
}
{
std::cerr
<< "exception caught while running periodical commands (after): "
<< str << std::endl;
}
catch (...)
{
std::cerr
<< "unknown exception caught while"
<< " running periodical commands (after)" << std::endl;
}
{
std::cerr
<< "unknown exception caught while"
<< " running periodical commands (after)" << std::endl;
}
// Others signals.
......@@ -376,11 +376,11 @@ void Device::integrate( const double & dt )
if (secondOrderIntegration_)
{
for( unsigned int i=0;i<control.size();++i )
{
if(control.size() == velocity_.size()) offset = 0;
vel_control_(i) = velocity_(i+offset) + control(i)*dt*0.5;
velocity_(i+offset) = velocity_(i+offset) + control(i)*dt;
}
{
if(control.size() == velocity_.size()) offset = 0;
vel_control_(i) = velocity_(i+offset) + control(i)*dt*0.5;
velocity_(i+offset) = velocity_(i+offset) + control(i)*dt;
}
}
else
{
......@@ -388,9 +388,9 @@ void Device::integrate( const double & dt )
}
if (vel_control_.size() == state_.size()) {
offset = 0;
integrateRollPitchYaw(state_, vel_control_, dt);
}
offset = 0;
integrateRollPitchYaw(state_, vel_control_, dt);
}
for( unsigned int i=6;i<state_.size();++i )
{ state_(i) += (vel_control_(i-offset)*dt); }
......
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