Commit da269cd8 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Compiling version.

parent af1bad3f
Pipeline #2043 failed with stage
in 6 minutes and 38 seconds
...@@ -101,9 +101,20 @@ namespace dynamicgraph { ...@@ -101,9 +101,20 @@ namespace dynamicgraph {
struct IMUSOUT struct IMUSOUT
{ {
std::string imu_sensor_name;
dg::Signal<MatrixRotation, int> attitudeSOUT; dg::Signal<MatrixRotation, int> attitudeSOUT;
dg::Signal<dg::Vector,int> accelerometerSOUT; dg::Signal<dg::Vector,int> accelerometerSOUT;
dg::Signal<dg::Vector,int> gyrometerSOUT; dg::Signal<dg::Vector,int> gyrometerSOUT;
IMUSOUT(const std::string &limu_sensor_name,
const std::string &device_name):
imu_sensor_name(limu_sensor_name)
,attitudeSOUT("PinocchioDevice("+device_name+
")::output(vector6)::"+imu_sensor_name+"_attitudeSOUT")
,accelerometerSOUT("PinocchioDevice("+device_name+
")::output(vector3)::"+imu_sensor_name+"_accelerometerSOUT")
,gyrometerSOUT("PinocchioDevice("+device_name+
")::output(vector3)::"+imu_sensor_name+"_gyrometerSOUT")
{}
}; };
typedef std::map<std::string,JointSoTHWControlType>::iterator typedef std::map<std::string,JointSoTHWControlType>::iterator
JointSHWControlType_iterator; JointSHWControlType_iterator;
...@@ -119,8 +130,11 @@ namespace dynamicgraph { ...@@ -119,8 +130,11 @@ namespace dynamicgraph {
virtual const std::string& getClassName(void) const { virtual const std::string& getClassName(void) const {
return CLASS_NAME; return CLASS_NAME;
} }
static const double TIMESTEP_DEFAULT;
protected: protected:
/// \brief Current integration step.
double timestep_;
/// \name Vectors related to the state. /// \name Vectors related to the state.
///@{ ///@{
...@@ -202,7 +216,7 @@ namespace dynamicgraph { ...@@ -202,7 +216,7 @@ namespace dynamicgraph {
/// \brief Output integrated state from control. /// \brief Output integrated state from control.
dg::Signal<dg::Vector,int> stateSOUT_; dg::Signal<dg::Vector,int> stateSOUT_;
/// \brief Output integrated velocity from control /// \brief Output integrated velocity from control
dg::Signal<dg::Vector,int> *velocitySOUT_; dg::Signal<dg::Vector,int> velocitySOUT_;
/// \brief Output attitude provided by the hardware /// \brief Output attitude provided by the hardware
/*! \brief The current state of the robot from the command viewpoint. */ /*! \brief The current state of the robot from the command viewpoint. */
dg::Signal<dg::Vector,int> motorcontrolSOUT_; dg::Signal<dg::Vector,int> motorcontrolSOUT_;
...@@ -221,17 +235,21 @@ namespace dynamicgraph { ...@@ -221,17 +235,21 @@ namespace dynamicgraph {
/// The force torque sensors /// The force torque sensors
std::vector<dg::Signal<dg::Vector,int>*> forcesSOUT_; std::vector<dg::Signal<dg::Vector,int>*> forcesSOUT_;
/// The imu sensors /// The imu sensors
std::vector<dg::Signal<dg::Vector, int>*> imuSOUT_; std::vector<IMUSOUT *> imuSOUT_;
/// Motor or pseudo torques (estimated or build from PID) /// Motor or pseudo torques (estimated or build from PID)
dg::Signal<dg::Vector,int> * pseudoTorqueSOUT_; dg::Signal<dg::Vector,int> * pseudoTorqueSOUT_;
/// Temperature signal /// Temperature signal
dg::Signal<dg::Vector,int> * temperatureSOUT_; dg::Signal<dg::Vector,int> * temperatureSOUT_;
/// Current signal /// Current signal
dg::Signal<dg::Vector,int> * currentsSOUT_; dg::Signal<dg::Vector,int> * currentsSOUT_;
/// Motor angles signal; /// Motor angles signal
dg::Signal<dg::Vector, int> * motor_anglesSOUT_; dg::Signal<dg::Vector, int> * motor_anglesSOUT_;
/// Joint angles signal; /// Joint angles signal
dg::Signal<dg::Vector, int> * joint_anglesSOUT_; dg::Signal<dg::Vector, int> * joint_anglesSOUT_;
/// P gains signal
dg::Signal<dg::Vector,int> * p_gainsSOUT_;
/// D gains signal
dg::Signal<dg::Vector,int> * d_gainsSOUT_;
/// \} /// \}
/// Parse a YAML string for configuration. /// Parse a YAML string for configuration.
...@@ -251,6 +269,8 @@ namespace dynamicgraph { ...@@ -251,6 +269,8 @@ namespace dynamicgraph {
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut); void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
///@} ///@}
protected: protected:
void setControlType(const std::string &strCtrlType, void setControlType(const std::string &strCtrlType,
ControlType &aCtrlType); ControlType &aCtrlType);
...@@ -316,6 +336,7 @@ namespace dynamicgraph { ...@@ -316,6 +336,7 @@ namespace dynamicgraph {
virtual void setRoot( const MatrixHomogeneous & worldMwaist ); virtual void setRoot( const MatrixHomogeneous & worldMwaist );
private: private:
// Intermediate variable to avoid dynamic allocation // Intermediate variable to avoid dynamic allocation
dg::Vector forceZero6; dg::Vector forceZero6;
...@@ -328,8 +349,8 @@ namespace dynamicgraph { ...@@ -328,8 +349,8 @@ namespace dynamicgraph {
// Intermediate index when parsing YAML file. // Intermediate index when parsing YAML file.
int temperature_index_,velocity_index_, int temperature_index_,velocity_index_,
current_index_,torque_index_, current_index_,torque_index_,
joint_angles_index_, joint_angle_index_,
motor_angles_index_ motor_angle_index_
; ;
public: public:
const se3::Model & getModel() const se3::Model & getModel()
......
...@@ -29,6 +29,30 @@ using namespace std; ...@@ -29,6 +29,30 @@ using namespace std;
using namespace dynamicgraph::sot; using namespace dynamicgraph::sot;
using namespace dynamicgraph; using namespace dynamicgraph;
#define DBGFILE "/tmp/sot-talos-device.txt"
#if 0
#define RESETDEBUG5() { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::out); \
DebugFile.close();}
#define ODEBUG5FULL(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG5(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#else
// Void the macro
#define RESETDEBUG5()
#define ODEBUG5FULL(x)
#define ODEBUG5(x)
#endif
const std::string PinocchioDevice::CLASS_NAME = "PinocchioDevice"; const std::string PinocchioDevice::CLASS_NAME = "PinocchioDevice";
...@@ -66,18 +90,28 @@ PinocchioDevice:: ...@@ -66,18 +90,28 @@ PinocchioDevice::
} }
const double PinocchioDevice::TIMESTEP_DEFAULT = 0.001;
PinocchioDevice:: PinocchioDevice::
PinocchioDevice( const std::string& n ) PinocchioDevice( const std::string& n )
:Entity(n) :Entity(n)
,timestep_(TIMESTEP_DEFAULT)
,position_(6) ,position_(6)
,sanityCheck_(true) ,sanityCheck_(true)
,controlSIN( NULL,"PinocchioDevice("+n+")::input(double)::control" ) ,controlSIN( NULL,"PinocchioDevice("+n+")::input(double)::control" )
,stateSOUT_( "PinocchioDevice("+n+")::output(vector)::state" ) ,stateSOUT_( "PinocchioDevice("+n+")::output(vector)::state" )
,velocitySOUT_( "PinocchioDevice("+n+")::output(vector)::velocity" ) ,velocitySOUT_("PinocchioDevice("+n+")::output(vector)::velocity" )
,motorcontrolSOUT_ ( "PinocchioDevice("+n+")::output(vector)::motorcontrol" ) ,motorcontrolSOUT_ ( "PinocchioDevice("+n+")::output(vector)::motorcontrol" )
,previousControlSOUT_( "PinocchioDevice("+n+")::output(vector)::previousControl" ) ,previousControlSOUT_( "PinocchioDevice("+n+")::output(vector)::previousControl" )
,robotState_ ("PinocchioDevice("+n+")::output(vector)::robotState") ,robotState_ ("PinocchioDevice("+n+")::output(vector)::robotState")
,robotVelocity_ ("PinocchioDevice("+n+")::output(vector)::robotVelocity") ,robotVelocity_ ("PinocchioDevice("+n+")::output(vector)::robotVelocity")
,forcesSOUT_(0)
,imuSOUT_(0)
,pseudoTorqueSOUT_(0)
,temperatureSOUT_(0)
,currentsSOUT_(0)
,motor_anglesSOUT_(0)
,joint_anglesSOUT_(0)
,ffPose_() ,ffPose_()
,forceZero6 (6) ,forceZero6 (6)
,debug_mode_(5) ,debug_mode_(5)
...@@ -85,8 +119,8 @@ PinocchioDevice( const std::string& n ) ...@@ -85,8 +119,8 @@ PinocchioDevice( const std::string& n )
,velocity_index_(0) ,velocity_index_(0)
,current_index_(0) ,current_index_(0)
,torque_index_(0) ,torque_index_(0)
,joint_angles_index_(0) ,joint_angle_index_(0)
,motor_angles_index_(0) ,motor_angle_index_(0)
{ {
forceZero6.fill (0); forceZero6.fill (0);
...@@ -516,13 +550,13 @@ ParseYAMLJointSensor(std::string & joint_name, ...@@ -516,13 +550,13 @@ ParseYAMLJointSensor(std::string & joint_name,
} }
else if (aSensor=="joint_angles") else if (aSensor=="joint_angles")
{ {
aJointSoTHWControlType.joint_angle_index = joint_angles_index_; aJointSoTHWControlType.joint_angle_index = joint_angle_index_;
joint_angles_index_++; joint_angle_index_++;
} }
else if (aSensor=="motor_angles") else if (aSensor=="motor_angles")
{ {
aJointSoTHWControlType.motor_angle_index = motor_angles_index_; aJointSoTHWControlType.motor_angle_index = motor_angle_index_;
motor_angles_index_++; motor_angle_index_++;
} }
} }
...@@ -672,13 +706,13 @@ void PinocchioDevice::CreateAForceSignal(const std::string & force_sensor_name) ...@@ -672,13 +706,13 @@ void PinocchioDevice::CreateAForceSignal(const std::string & force_sensor_name)
void PinocchioDevice::CreateAnImuSignal(const std::string &imu_sensor_name) void PinocchioDevice::CreateAnImuSignal(const std::string &imu_sensor_name)
{ {
dynamicgraph::Signal<dg::Vector, int> * anImuSOUT_; IMUSOUT * anImuSOUT_;
/* --- SIGNALS --- */ /* --- SIGNALS --- */
anImuSOUT_ = anImuSOUT_ = new IMUSOUT(imu_sensor_name,getName());
new Signal<Vector, int>("PinocchioDevice("+getName()+")::output(vector6)::" +
imu_sensor_name);
imuSOUT_.push_back(anImuSOUT_); imuSOUT_.push_back(anImuSOUT_);
signalRegistration(*anImuSOUT_); signalRegistration(anImuSOUT_->attitudeSOUT
<< anImuSOUT_->accelerometerSOUT
<< anImuSOUT_->gyrometerSOUT);
} }
...@@ -700,9 +734,8 @@ UpdateSignals() ...@@ -700,9 +734,8 @@ UpdateSignals()
joint_anglesSOUT_ = new Signal<Vector, int> joint_anglesSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::joint_angles"); ("PinocchioDevice("+getName()+")::output(vector)::joint_angles");
velocitySOUT_ = new Signal<Vector, int> return 0;
("PinocchioDevice("+getName()+")::output(vector)::joint_velocities");
} }
...@@ -713,9 +746,12 @@ void PinocchioDevice::display ( std::ostream& os ) const ...@@ -713,9 +746,12 @@ void PinocchioDevice::display ( std::ostream& os ) const
{os <<name<<": "<<position_<<endl; } {os <<name<<": "<<position_<<endl; }
/* Helpers for the controller */ /* Helpers for the controller */
void PinocchioDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, void PinocchioDevice::
int t) setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn,
int t)
{ {
Eigen::Matrix<double, 6, 1> dgforces;
sotDEBUGIN(15); sotDEBUGIN(15);
map<string,dgsot::SensorValues>::iterator it; map<string,dgsot::SensorValues>::iterator it;
it = SensorsIn.find("forces"); it = SensorsIn.find("forces");
...@@ -724,38 +760,43 @@ void PinocchioDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn ...@@ -724,38 +760,43 @@ void PinocchioDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn
// Implements force recollection. // Implements force recollection.
const vector<double>& forcesIn = it->second.getValues(); const vector<double>& forcesIn = it->second.getValues();
for(int i=0;i<forcesSOUT_.size();++i) for(std::size_t i=0;i<forcesSOUT_.size();++i)
{ {
sotDEBUG(15) << "Force sensor " << i << std::endl; sotDEBUG(15) << "Force sensor " << i << std::endl;
for(int j=0;j<6;++j) for(int j=0;j<6;++j)
{ {
dgforces_(j) = forcesIn[i*6+j]; dgforces(j) = forcesIn[i*6+j];
sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl; sotDEBUG(15) << "Force value " << j << ":" << dgforces(j) << std::endl;
} }
forcesSOUT_[i]->setConstant(dgforces_); forcesSOUT_[i]->setConstant(dgforces);
forcesSOUT_[i]->setTime (t); forcesSOUT_[i]->setTime (t);
} }
} }
sotDEBUGIN(15); sotDEBUGIN(15);
} }
void PinocchioDevice::setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn, void PinocchioDevice::
int t) setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn,
int t)
{ {
Eigen::Matrix<double,3, 1> aVector3d;
map<string,dgsot::SensorValues>::iterator it; map<string,dgsot::SensorValues>::iterator it;
//TODO: Confirm if this can be made quaternion //TODO: Confirm if this can be made quaternion
for(int i=0;i<imuSOUT_.size();++i) for(std::size_t k=0;k<imuSOUT_.size();++k)
{ {
it = SensorsIn.find("attitude"); it = SensorsIn.find("attitude");
if (it!=SensorsIn.end()) if (it!=SensorsIn.end())
{ {
const vector<double>& attitude = it->second.getValues (); const vector<double>& attitude = it->second.getValues ();
Eigen::Matrix<double, 3, 3> pose;
for (unsigned int i = 0; i < 3; ++i) for (unsigned int i = 0; i < 3; ++i)
for (unsigned int j = 0; j < 3; ++j) for (unsigned int j = 0; j < 3; ++j)
pose (i, j) = attitude [i * 3 + j]; pose (i, j) = attitude [i * 3 + j];
imuSOUT_[i].attitudeSOUT_.setConstant (pose); imuSOUT_[k]->attitudeSOUT.setConstant (pose);
imuSOUT_[i].attitudeSOUT_.setTime (t); imuSOUT_[k]->attitudeSOUT.setTime (t);
} }
it = SensorsIn.find("accelerometer_0"); it = SensorsIn.find("accelerometer_0");
...@@ -764,9 +805,9 @@ void PinocchioDevice::setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn, ...@@ -764,9 +805,9 @@ void PinocchioDevice::setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn,
const vector<double>& accelerometer = const vector<double>& accelerometer =
SensorsIn ["accelerometer_0"].getValues (); SensorsIn ["accelerometer_0"].getValues ();
for (std::size_t i=0; i<3; ++i) for (std::size_t i=0; i<3; ++i)
accelerometer_ (i) = accelerometer [i]; aVector3d(i) = accelerometer [i];
imuSOUT_[i].accelerometerSOUT_.setConstant (accelerometer_); imuSOUT_[k]->accelerometerSOUT.setConstant (aVector3d);
imuSOUT_[i].accelerometerSOUT_.setTime (t); imuSOUT_[k]->accelerometerSOUT.setTime (t);
} }
it = SensorsIn.find("gyrometer_0"); it = SensorsIn.find("gyrometer_0");
...@@ -774,49 +815,55 @@ void PinocchioDevice::setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn, ...@@ -774,49 +815,55 @@ void PinocchioDevice::setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn,
{ {
const vector<double>& gyrometer = SensorsIn ["gyrometer_0"].getValues (); const vector<double>& gyrometer = SensorsIn ["gyrometer_0"].getValues ();
for (std::size_t i=0; i<3; ++i) for (std::size_t i=0; i<3; ++i)
gyrometer_ (i) = gyrometer [i]; aVector3d(i) = gyrometer [i];
imuSOUT_[i].gyrometerSOUT_.setConstant (gyrometer_); imuSOUT_[k]->gyrometerSOUT.setConstant (aVector3d);
imuSOUT_[i].gyrometerSOUT_.setTime (t); imuSOUT_[k]->gyrometerSOUT.setTime (t);
} }
} }
}
} }
void PinocchioDevice:: void PinocchioDevice::
setSensorsEncoders(map<string,dgsot::SensorValues> &SensorsIn, setSensorsEncoders(map<string,dgsot::SensorValues> &SensorsIn,
int t) int t)
{ {
dg::Vector dgRobotState, motor_angles, joint_angles;
map<string,dgsot::SensorValues>::iterator it; map<string,dgsot::SensorValues>::iterator it;
it = SensorsIn.find("motor-angles");
if (it!=SensorsIn.end())
{
const vector<double>& anglesIn = it->second.getValues();
dgRobotState_.resize (anglesIn.size () + 6);
motor_angles_.resize(anglesIn.size ());
for (unsigned i = 0; i < 6; ++i)
dgRobotState_ (i) = 0.;
for (unsigned i = 0; i < anglesIn.size(); ++i)
{
dgRobotState_ (i + 6) = anglesIn[i];
motor_angles_(i)= anglesIn[i];
}
robotState_.setConstant(dgRobotState_);
robotState_.setTime(t);
motor_anglesSOUT_.setConstant(motor_angles_);
motor_anglesSOUT_.setTime(t);
}
it = SensorsIn.find("joint-angles"); if (motor_anglesSOUT_!=0)
if (it!=SensorsIn.end()) {
{ it = SensorsIn.find("motor-angles");
const vector<double>& joint_anglesIn = it->second.getValues(); if (it!=SensorsIn.end())
joint_angles_.resize (joint_anglesIn.size () ); {
for (unsigned i = 0; i < joint_anglesIn.size(); ++i) const vector<double>& anglesIn = it->second.getValues();
joint_angles_ (i) = joint_anglesIn[i]; dgRobotState.resize (anglesIn.size () + 6);
joint_anglesSOUT_.setConstant(joint_angles_); motor_angles.resize(anglesIn.size ());
joint_anglesSOUT_.setTime(t); for (unsigned i = 0; i < 6; ++i)
} dgRobotState (i) = 0.;
for (unsigned i = 0; i < anglesIn.size(); ++i)
{
dgRobotState (i + 6) = anglesIn[i];
motor_angles(i)= anglesIn[i];
}
robotState_.setConstant(dgRobotState);
robotState_.setTime(t);
motor_anglesSOUT_->setConstant(motor_angles);
motor_anglesSOUT_->setTime(t);
}
}
if (joint_anglesSOUT_!=0)
{
it = SensorsIn.find("joint-angles");
if (it!=SensorsIn.end())
{
const vector<double>& joint_anglesIn = it->second.getValues();
joint_angles.resize (joint_anglesIn.size () );
for (unsigned i = 0; i < joint_anglesIn.size(); ++i)
joint_angles (i) = joint_anglesIn[i];
joint_anglesSOUT_->setConstant(joint_angles);
joint_anglesSOUT_->setTime(t);
}
}
} }
...@@ -824,20 +871,22 @@ void PinocchioDevice:: ...@@ -824,20 +871,22 @@ void PinocchioDevice::
setSensorsVelocities(map<string,dgsot::SensorValues> &SensorsIn, setSensorsVelocities(map<string,dgsot::SensorValues> &SensorsIn,
int t) int t)
{ {
dg::Vector dgRobotVelocity;
map<string,dgsot::SensorValues>::iterator it; map<string,dgsot::SensorValues>::iterator it;
it = SensorsIn.find("velocities"); it = SensorsIn.find("velocities");
if (it!=SensorsIn.end()) if (it!=SensorsIn.end())
{ {
const vector<double>& velocitiesIn = it->second.getValues(); const vector<double>& velocitiesIn = it->second.getValues();
dgRobotVelocity_.resize (velocitiesIn.size () + 6); dgRobotVelocity.resize (velocitiesIn.size () + 6);
for (unsigned i = 0; i < 6; ++i) for (unsigned i = 0; i < 6; ++i)
dgRobotVelocity_ (i) = 0.; dgRobotVelocity (i) = 0.;
for (unsigned i = 0; i < velocitiesIn.size(); ++i) for (unsigned i = 0; i < velocitiesIn.size(); ++i)
{ {
dgRobotVelocity_ (i + 6) = velocitiesIn[i]; dgRobotVelocity (i + 6) = velocitiesIn[i];
} }
robotVelocity_.setConstant(dgRobotVelocity_); robotVelocity_.setConstant(dgRobotVelocity);
robotVelocity_.setTime(t); robotVelocity_.setTime(t);
} }
...@@ -845,55 +894,72 @@ setSensorsVelocities(map<string,dgsot::SensorValues> &SensorsIn, ...@@ -845,55 +894,72 @@ setSensorsVelocities(map<string,dgsot::SensorValues> &SensorsIn,
void PinocchioDevice::setSensorsTorquesCurrents(map<string,dgsot::SensorValues> &SensorsIn, int t) void PinocchioDevice::setSensorsTorquesCurrents(map<string,dgsot::SensorValues> &SensorsIn, int t)
{ {
map<string,dgsot::SensorValues>::iterator it; dg::Vector torques,currents;
it = SensorsIn.find("torques");
if (it!=SensorsIn.end())
{
const std::vector<double>& torques = SensorsIn["torques"].getValues();
torques_.resize(torques.size());
for(std::size_t i = 0; i < torques.size(); ++i)
torques_ (i) = torques [i];
pseudoTorqueSOUT_.setConstant(torques_);
pseudoTorqueSOUT_.setTime(t);
}
it = SensorsIn.find("currents"); map<string,dgsot::SensorValues>::iterator it;
if (it!=SensorsIn.end())
{ if (pseudoTorqueSOUT_!=0)
const std::vector<double>& currents = SensorsIn["currents"].getValues(); {
currents_.resize(currents.size()); it = SensorsIn.find("torques");
for(std::size_t i = 0; i < currents.size(); ++i) if (it!=SensorsIn.end())
currents_ (i) = currents[i]; {
currentsSOUT_.setConstant(currents_); const std::vector<double>& vtorques = SensorsIn["torques"].getValues();
currentsSOUT_.setTime(t); torques.resize(vtorques.size());
} for(std::size_t i = 0; i < vtorques.size(); ++i)
torques (i) = vtorques [i];
pseudoTorqueSOUT_->setConstant(torques);
pseudoTorqueSOUT_->setTime(t);
}
}
if (currentsSOUT_!=0)
{
it = SensorsIn.find("currents");
if (it!=SensorsIn.end())
{
const std::vector<double>& vcurrents = SensorsIn["currents"].getValues();
currents.resize(vcurrents.size());
for(std::size_t i = 0; i < vcurrents.size(); ++i)
currents (i) = vcurrents[i];
currentsSOUT_->setConstant(currents);
currentsSOUT_->setTime(t);
}
}
} }