roscontrol-sot-controller.cpp 36 KB
Newer Older
Olivier Stasse's avatar
Olivier Stasse committed
1
2
3
4
5
#include <fstream>
#include <iomanip>
#include <dlfcn.h>
#include <sstream>

6

Olivier Stasse's avatar
Olivier Stasse committed
7
8
#include "roscontrol-sot-controller.hh"

9
#include<ros/console.h>
Olivier Stasse's avatar
Olivier Stasse committed
10

11
12
13
#define ENABLE_RT_LOG
#include<dynamic-graph/real-time-logger.h>

Olivier Stasse's avatar
Olivier Stasse committed
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl

#define DBGFILE "/tmp/rcoh2sot.dat"

#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();}

37
#define RESETDEBUG4()
38
39
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
Olivier Stasse's avatar
Olivier Stasse committed
40

41
42
43
44
class LoggerROSStream : public ::dynamicgraph::LoggerStream
{
  public:
    void write (const char* c) {
Olivier Stasse's avatar
Olivier Stasse committed
45
      ROS_ERROR ("%s",c);
46
47
48
    }
};

49
50
51
52
53
54
/// lhi: nickname for local_hardware_interface
/// Depends if we are on the real robot or not.

namespace lhi=hardware_interface;
using namespace lhi;

Olivier Stasse's avatar
Olivier Stasse committed
55
56
using namespace rc_sot_system;

57
namespace sot_controller  
Olivier Stasse's avatar
Olivier Stasse committed
58
59
{
  typedef std::map<std::string,std::string>::iterator it_map_rt_to_sot;
60
61
  typedef std::map<std::string,std::string>::iterator it_control_mode;
  
62
  ControlPDMotorControlData::ControlPDMotorControlData()
63
64
  {
  }
Olivier Stasse's avatar
Olivier Stasse committed
65
  
66
  void ControlPDMotorControlData::read_from_xmlrpc_value
67
68
69
70
71
  (const std::string &prefix)
  {
    pid_controller.initParam(prefix);
  }
   
Olivier Stasse's avatar
Olivier Stasse committed
72
73
74
75
76
77
  RCSotController::
  RCSotController():
    // Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000)
    // -> 124 Mo of data.
    type_name_("RCSotController"),
    simulation_mode_(false),
78
    accumulated_time_(0.0),
79
80
    jitter_(0.0),
    verbosity_level_(0)
Olivier Stasse's avatar
Olivier Stasse committed
81
  {
82
    RESETDEBUG4();
83
    profileLog_.length=300000;
Olivier Stasse's avatar
Olivier Stasse committed
84
85
  }
  
86
  void RCSotController::
87
  displayClaimedResources(ClaimedResources & claimed_resources)
88
  {
89
90
#ifdef CONTROLLER_INTERFACE_KINETIC
    ClaimedResources::iterator it_claim;
91
92
93
94
95
    ROS_INFO_STREAM("Size of claimed resources: "<< claimed_resources.size());
    for (it_claim = claimed_resources.begin(); 
	 it_claim != claimed_resources.end(); 
	 ++it_claim)
      {
96
97
98
99
100
101
102
103
104
105
106
107
	hardware_interface::InterfaceResources & aclaim = *it_claim;
	ROS_INFO_STREAM("Claimed by RCSotController: " << aclaim.hardware_interface);
	
	for(std::set<std::string>::iterator
	      it_set_res=aclaim.resources.begin();
	    it_set_res!=aclaim.resources.end();
	    it_set_res++)
	  {
	    ROS_INFO_STREAM(" Resources belonging to the interface:" <<
			    *it_set_res);
	  }
	    
108
      }
109
110
111
112
113
114
115
116
117
118
119
#else
    std::set<std::string >::iterator it_claim;
    ROS_INFO_STREAM("Size of claimed resources: "<< claimed_resources.size());
    for (it_claim = claimed_resources.begin();
	 it_claim != claimed_resources.end();
	 ++it_claim)
      {
	std::string aclaim = *it_claim;
	ROS_INFO_STREAM("Claimed by RCSotController: " << aclaim);
      }
#endif
120
121
  }

122
123
124
  void RCSotController::initLogs()
  {
    ROS_INFO_STREAM("Initialize log data structure");
125
126
127
    /// Initialize the size of the data to store.
    /// Set temporary profileLog to one
    /// because DataOneIter is just for one iteration.
128
    size_t tmp_length = profileLog_.length;
129
    profileLog_.length = 1;
130
    DataOneIter_.init(profileLog_);
131
132
133

    /// Set profile Log to real good value for the stored data.
    profileLog_.length= tmp_length;
134
    /// Initialize the data logger for 300s.
135
    RcSotLog_.init(profileLog_);
136
137
138

  }
  
Olivier Stasse's avatar
Olivier Stasse committed
139
  bool RCSotController::
140
  initRequest (lhi::RobotHW * robot_hw,
Olivier Stasse's avatar
Olivier Stasse committed
141
142
	       ros::NodeHandle &robot_nh,
	       ros::NodeHandle &controller_nh,
143
	       ClaimedResources & claimed_resources)
Olivier Stasse's avatar
Olivier Stasse committed
144
145
146
147
148
149
  {
    /// Read the parameter server
    if (!readParams(robot_nh))
      return false;

    /// Create ros control interfaces to hardware
150
    /// Recalls: init() is called by initInterfaces()
151
    if (!initInterfaces(robot_hw,robot_nh,controller_nh,claimed_resources))
Olivier Stasse's avatar
Olivier Stasse committed
152
153
      return false;

154
155
156
    /// Create all the internal data structures for logging.
    initLogs();
    
Olivier Stasse's avatar
Olivier Stasse committed
157
158
159
    /// Create SoT
    SotLoaderBasic::Initialization();

160
161
    /// Fill desired position during the phase where the robot is waiting.
    for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
162
      {
163
	std::string joint_name = joints_name_[idJoint];
164
	std::map<std::string,ControlPDMotorControlData>::iterator
165
166
167
	  search_ecpd = effort_mode_pd_motors_.find(joint_name);
	
	if (search_ecpd!=effort_mode_pd_motors_.end())
168
	  {
169
170
171
	    /// If we are in effort mode then the device should not do any integration.
	    sotController_->setNoIntegration();

172
173
	  }
      }
Olivier Stasse's avatar
Olivier Stasse committed
174
175
176
177
    return true;
  }
  
  bool RCSotController::
178
  initInterfaces(lhi::RobotHW * robot_hw,
Olivier Stasse's avatar
Olivier Stasse committed
179
180
		 ros::NodeHandle &,
		 ros::NodeHandle &,
181
		 ClaimedResources & claimed_resources)
Olivier Stasse's avatar
Olivier Stasse committed
182
  {
183
184
185
    std::string lns;
    lns="hardware_interface";

Olivier Stasse's avatar
Olivier Stasse committed
186
187
188
189
190
191
    // Check if construction finished cleanly
    if (state_!=CONSTRUCTED)
      {
	ROS_ERROR("Cannot initialize this controller because it failed to be constructed");
      }

192
193
    // Get a pointer to the joint position control interface
    pos_iface_ = robot_hw->get<PositionJointInterface>();
194
    if (!pos_iface_)
Olivier Stasse's avatar
Olivier Stasse committed
195
      {
196
197
198
	ROS_WARN("This controller did not find  a hardware interface of type PositionJointInterface."
		 " Make sure this is registered in the %s::RobotHW class if it is required."
		 , lns.c_str());
Olivier Stasse's avatar
Olivier Stasse committed
199
      }
200

Joseph Mirabel's avatar
Joseph Mirabel committed
201
202
203
204
205
206
207
208
209
    // Get a pointer to the joint velocity control interface
    vel_iface_ = robot_hw->get<VelocityJointInterface>();
    if (!vel_iface_)
      {
	ROS_WARN("This controller did not find  a hardware interface of type VelocityJointInterface."
		 " Make sure this is registered in the %s::RobotHW class if it is required."
		 , lns.c_str());
      }

210
211
212
    // Get a pointer to the joint effort control interface
    effort_iface_ = robot_hw->get<EffortJointInterface>();
    if (! effort_iface_)
Olivier Stasse's avatar
Olivier Stasse committed
213
      {
214
	ROS_WARN("This controller did not find a hardware interface of type EffortJointInterface."
215
		 " Make sure this is registered in the %s::RobotHW class if it is required.",
216
		 lns.c_str());
Olivier Stasse's avatar
Olivier Stasse committed
217
218
      }

219
220
221
222
    // Get a pointer to the force-torque sensor interface
    ft_iface_ = robot_hw->get<ForceTorqueSensorInterface>();
    if (! ft_iface_ )
      {
223
224
	ROS_WARN("This controller did not find a hardware interface of type '%s '. " 
		 " Make sure this is registered inthe %s::RobotHW class if it is required.",
225
226
		  internal :: demangledTypeName<ForceTorqueSensorInterface>().c_str(),lns.c_str());
      }
227
    
Olivier Stasse's avatar
Olivier Stasse committed
228
229
230
231
    // Get a pointer to the IMU sensor interface
    imu_iface_ = robot_hw->get<ImuSensorInterface>();
    if (! imu_iface_)
      {
232
233
234
	ROS_WARN("This controller did not find a hardware interface of type '%s'."
		 " Make sure this is registered in the %s::RobotHW class if it is required.",
		    internal :: demangledTypeName<ImuSensorInterface>().c_str(),lns.c_str());
Olivier Stasse's avatar
Olivier Stasse committed
235
      }
236

237
238
239
    // Temperature sensor not available in simulation mode
    if (!simulation_mode_)
      {
240
#ifdef TEMPERATURE_SENSOR_CONTROLLER
241
242
243
244
	// Get a pointer to the actuator temperature sensor interface
	act_temp_iface_ = robot_hw->get<ActuatorTemperatureSensorInterface>();
	if (!act_temp_iface_)
	  {
245
246
	    ROS_WARN("This controller did not find a hardware interface of type '%s'."
		     " Make sure this is registered in the %s::RobotHW class if it is required.",
247
248
		      internal :: demangledTypeName<ActuatorTemperatureSensorInterface>().c_str(),lns.c_str());
	  }
249
#endif
250
251
      }
	
Olivier Stasse's avatar
Olivier Stasse committed
252
253
      
    // Return which resources are claimed by this controller
254
    pos_iface_->clearClaims();
Joseph Mirabel's avatar
Joseph Mirabel committed
255
    vel_iface_->clearClaims();
256
    effort_iface_->clearClaims();
Olivier Stasse's avatar
Olivier Stasse committed
257
    
258
    if (! init())
Olivier Stasse's avatar
Olivier Stasse committed
259
260
261
262
263
      {
	ROS_ERROR("Failed to initialize sot-controller" );
	std :: cerr << "FAILED LOADING SOT CONTROLLER" << std::endl;
	return false ;
      }
264
265
    if (verbosity_level_>0)
      ROS_INFO_STREAM("Initialization of interfaces for sot-controller Ok !");
Olivier Stasse's avatar
Olivier Stasse committed
266
267

#ifdef CONTROLLER_INTERFACE_KINETIC
268
    hardware_interface::InterfaceResources iface_res;
269
270
271
    iface_res.hardware_interface =
      hardware_interface::internal::
      demangledTypeName<PositionJointInterface>();
272
273
    iface_res.resources = pos_iface_->getClaims();
    claimed_resources.push_back(iface_res);
Joseph Mirabel's avatar
Joseph Mirabel committed
274
275
276
277
278
279

    iface_res.hardware_interface =
      hardware_interface::internal::
      demangledTypeName<VelocityJointInterface>();
    iface_res.resources = vel_iface_->getClaims();
    claimed_resources.push_back(iface_res);
Olivier Stasse's avatar
Olivier Stasse committed
280

281
282
283
    iface_res.hardware_interface =
      hardware_interface::internal::
      demangledTypeName<EffortJointInterface>();
284
285
    iface_res.resources = effort_iface_->getClaims();
    claimed_resources.push_back(iface_res);
Joseph Mirabel's avatar
Joseph Mirabel committed
286
287

    /// Display claimed ressources
288
289
    if (verbosity_level_>0)
      displayClaimedResources(claimed_resources);
Joseph Mirabel's avatar
Joseph Mirabel committed
290
291
292

    pos_iface_->clearClaims();
    vel_iface_->clearClaims();
293
    effort_iface_->clearClaims();
Olivier Stasse's avatar
Olivier Stasse committed
294
295
#else
    claimed_resources = pos_iface_->getClaims();
296
297
298
    /// Display claimed ressources
    if (verbosity_level_>0)
      displayClaimedResources(claimed_resources);
Olivier Stasse's avatar
Olivier Stasse committed
299
300
    pos_iface_->clearClaims();

Joseph Mirabel's avatar
Joseph Mirabel committed
301
302
303
304
305
306
    claimed_resources = vel_iface_->getClaims();
    /// Display claimed ressources
    if (verbosity_level_>0)
      displayClaimedResources(claimed_resources);
    vel_iface_->clearClaims();

Olivier Stasse's avatar
Olivier Stasse committed
307
    claimed_resources = effort_iface_->getClaims();
308
309
    if (verbosity_level_>0)
      displayClaimedResources(claimed_resources);
Olivier Stasse's avatar
Olivier Stasse committed
310
311
    effort_iface_->clearClaims();
#endif    
312
313
    if (verbosity_level_>0)
      ROS_INFO_STREAM("Initialization of sot-controller Ok !");
Olivier Stasse's avatar
Olivier Stasse committed
314
315
316
317
318
319
320
321
322
323
324
    // success
    state_ = INITIALIZED;

    return true;
  }
    
  bool RCSotController::
  init()
  {
    if (!initJoints()) 
      return false;
325
326
327
328
329
330
331
332
333
    if (!initIMU()) {
      ROS_WARN("could not initialize IMU sensor(s).");
    }
    if (!initForceSensors()) {
      ROS_WARN("could not initialize force sensor(s).");
    }
    if (!initTemperatureSensors()) {
      ROS_WARN("could not initialize temperature sensor(s).");
    }
Olivier Stasse's avatar
Olivier Stasse committed
334
335
336
337
338
339
340
341
342
343
344

    // Initialize ros node.
    int argc=1;
    char *argv[1];
    argv[0] = new char[10];
    strcpy(argv[0],"libsot");
    SotLoaderBasic::initializeRosNode(argc,argv);
    
    return true;
  }

345
346
347
348
349
350
351
352
  void RCSotController::
  readParamsVerbosityLevel(ros::NodeHandle &robot_nh)
  {
    if (robot_nh.hasParam("/sot_controller/verbosity_level"))
      {
	robot_nh.getParam("/sot_controller/verbosity_level",verbosity_level_);
	ROS_INFO_STREAM("Verbosity_level " << verbosity_level_);
      }
353
354
355
356
357
358
359
360
    if (robot_nh.hasParam("/sot_controller/log/size"))
      {
	int llength;
	robot_nh.getParam("/sot_controller/log/size",llength);
	profileLog_.length=(unsigned int)llength;
	ROS_INFO_STREAM("Size of the log " << profileLog_.length);
      }

361
362
  }
  
Olivier Stasse's avatar
Olivier Stasse committed
363
364
365
366
367
368
369
370
371
  bool RCSotController::
  readParamsSotLibName(ros::NodeHandle &robot_nh)
  {
    // Read param to find the library to load
    std::string dynamic_library_name;

   // Read libname
    if (!robot_nh.getParam("/sot_controller/libname",dynamic_library_name))
      {
372
373
374
375
376
377
378
379
380
381
    	ROS_ERROR_STREAM("Could not read param /sot_controller/libname");
    	if (robot_nh.hasParam("/sot_controller/libname")) 
    	  {
    	    ROS_ERROR_STREAM("Param /sot_controller/libname exists !");
    	  }
    	else
    	  {
    	    ROS_ERROR_STREAM("Param /sot_controller/libname does not exists !");
    	    return false;
    	  }
Olivier Stasse's avatar
Olivier Stasse committed
382
383
384
      }
    else
      {
385
386
    	if (verbosity_level_>0)
    	  ROS_INFO_STREAM("Loading library name: " << dynamic_library_name);
Olivier Stasse's avatar
Olivier Stasse committed
387
388
389
390
391
392
393
      }
    /// SotLoaderBasic related method calls.
    // Initialize the dynamic_library_name for the sotLoader
    setDynamicLibraryName(dynamic_library_name);
    return true;
  }

394
  bool RCSotController::
395
  readParamsPositionControlData(ros::NodeHandle &)
396
397
398
399
  {
    return false;
  }
  
400
401
402
403
404
405
406
407
408
  bool RCSotController::
  readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh)
  {
    // Read libname
    if (robot_nh.hasParam("/sot_controller/effort_control_pd_motor_init/gains"))
      {
       XmlRpc::XmlRpcValue xml_rpc_ecpd_init;
       robot_nh.getParamCached("/sot_controller/effort_control_pd_motor_init/gains",
                               xml_rpc_ecpd_init);
409
410
411
412

       /// Display gain during transition control.
       if (verbosity_level_>0)
    	 ROS_INFO("/sot_controller/effort_control_pd_motor_init/gains: %d %d %d\n",
413
414
415
    		  xml_rpc_ecpd_init.getType(),
		  XmlRpc::XmlRpcValue::TypeArray,
		  XmlRpc::XmlRpcValue::TypeStruct);
416
417
418
419
420
       
       effort_mode_pd_motors_.clear();
       
       for (size_t i=0;i<joints_name_.size();i++)
         {
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
	   // Check if the joint should be in ROS EFFORT mode
	   std::map<std::string,JointSotHandle>::iterator
	     search_joint_sot_handle = joints_.find(joints_name_[i]);
	   if (search_joint_sot_handle!=joints_.end())
	     {
	       JointSotHandle aJointSotHandle = search_joint_sot_handle->second;
	       if (aJointSotHandle.ros_control_mode==EFFORT)
		 {
		   // Test if PID data is present
		   if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
		     {
		       std::string prefix=
			 "/sot_controller/effort_control_pd_motor_init/gains/"
			 + joints_name_[i];
		       effort_mode_pd_motors_[joints_name_[i]].
			 read_from_xmlrpc_value(prefix);
		     }
		   else
		     ROS_ERROR("No PID data for effort controlled joint %s in /sot_controller/effort_control_pd_motor_init/gains/",
			       joints_name_[i].c_str());
		 }
	     }

444
445
446
447
448
449
450
         }
       return true;
      }
    
    ROS_ERROR("No parameter /sot_controller/effort_controler_pd_motor_init");
    return false;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
  
  bool RCSotController::
  readParamsVelocityControlPDMotorControlData(ros::NodeHandle &robot_nh)
  {
    // Read libname
    if (robot_nh.hasParam("/sot_controller/velocity_control_pd_motor_init/gains"))
      {
       XmlRpc::XmlRpcValue xml_rpc_ecpd_init;
       robot_nh.getParamCached("/sot_controller/velocity_control_pd_motor_init/gains",
                               xml_rpc_ecpd_init);

       /// Display gain during transition control.
       if (verbosity_level_>0)
    	 ROS_INFO("/sot_controller/velocity_control_pd_motor_init/gains: %d %d %d\n",
    		  xml_rpc_ecpd_init.getType(),
		  XmlRpc::XmlRpcValue::TypeArray,
		  XmlRpc::XmlRpcValue::TypeStruct);
       
       velocity_mode_pd_motors_.clear();
       
       for (size_t i=0;i<joints_name_.size();i++)
         {
	   // Check if the joint should be in ROS VELOCITY mode
	   std::map<std::string,JointSotHandle>::iterator
	     search_joint_sot_handle = joints_.find(joints_name_[i]);
	   if (search_joint_sot_handle!=joints_.end())
	     {
	       JointSotHandle aJointSotHandle = search_joint_sot_handle->second;
	       if (aJointSotHandle.ros_control_mode==VELOCITY)
		 {
		   // Test if PID data is present
		   if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
		     {
		       std::string prefix=
			 "/sot_controller/velocity_control_pd_motor_init/gains/"
			 + joints_name_[i];
		       velocity_mode_pd_motors_[joints_name_[i]].
			 read_from_xmlrpc_value(prefix);
		     }
		   else
		     ROS_ERROR("No PID data for velocity controlled joint %s in /sot_controller/velocity_control_pd_motor_init/gains/",
			       joints_name_[i].c_str());
		 }
	     }

         }
       return true;
      }
    
    ROS_ERROR("No parameter /sot_controller/velocity_controler_pd_motor_init");
    return false;
  }
Olivier Stasse's avatar
Olivier Stasse committed
503
504
505
506
507
508
509

  bool RCSotController::
  readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh)
  {
    // Read libname
    if (robot_nh.hasParam("/sot_controller/map_rc_to_sot_device")) 
      {
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
    	if (robot_nh.getParam("/sot_controller/map_rc_to_sot_device",
    			      mapFromRCToSotDevice_))
    	  {
    	    /// TODO: Check if the mapping is complete wrt to the interface and the mapping.
    	    if (verbosity_level_>0)
    	      {
    		ROS_INFO_STREAM("Loading map rc to sot device: ");
    		for (it_map_rt_to_sot it = mapFromRCToSotDevice_.begin(); 
    		     it != mapFromRCToSotDevice_.end(); ++it) 
    		  ROS_INFO_STREAM( it->first << ", " << it->second);
    	      }
    	  }
    	else
    	  {
    	    ROS_ERROR_STREAM("Could not read param /sot_controller/map_rc_to_sot_device");
    	    return false;
    	  }
Olivier Stasse's avatar
Olivier Stasse committed
527
528
529
      }  
    else
      {
530
    	ROS_INFO_STREAM("Param /sot_controller/map_rc_to_sot_device does not exists !");
531
    	return false;
Olivier Stasse's avatar
Olivier Stasse committed
532
533
534
535
536
537
538
      }
    return true;
  }

  bool RCSotController::
  readParamsJointNames(ros::NodeHandle &robot_nh)
  {
539
    /// Check if the /sot_controller/joint_names parameter exists.
Olivier Stasse's avatar
Olivier Stasse committed
540
541
    if (robot_nh.hasParam("/sot_controller/joint_names")) 
      {
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
    	/// Read the joint_names list from this parameter
    	robot_nh.getParam("/sot_controller/joint_names",
    			  joints_name_);
    	for(std::vector<std::string>::size_type i=0;i<joints_name_.size();i++)
    	  {
    	    if (verbosity_level_>0)
    	      ROS_INFO_STREAM("joints_name_[" << i << "]=" << joints_name_[i]);

    	    if (modelURDF_.use_count())
    	      {
    		urdf::JointConstSharedPtr aJCSP = modelURDF_->getJoint(joints_name_[i]);
    		if (aJCSP.use_count()!=0)
    		  {
    		    if (verbosity_level_>0)
    		      ROS_INFO_STREAM( joints_name_[i] + " found in the robot model" );
    		  }
    		else
    		  {
Olivier Stasse's avatar
Olivier Stasse committed
560
    		    ROS_ERROR(" %s not found in the robot model",joints_name_[i].c_str());
561
562
563
564
565
566
567
    		    return false;
    		  }
    	      }
    	    else
    	      {
    		ROS_ERROR("No robot model loaded in /robot_description");
    		return false;
568
569
	      }
	  }
Olivier Stasse's avatar
Olivier Stasse committed
570
571
572
573
      }
    else
      return false;

574
    /// Deduce from this the degree of freedom number.
Olivier Stasse's avatar
Olivier Stasse committed
575
    nbDofs_ = joints_name_.size();
576
    profileLog_.nbDofs = nbDofs_;
Olivier Stasse's avatar
Olivier Stasse committed
577
578
579
580
581
	
    return true;
  }

  bool RCSotController::
582
583
  getJointControlMode(std::string &joint_name,
		      JointSotHandle &aJointSotHandle)
Olivier Stasse's avatar
Olivier Stasse committed
584
  {
585
586
    std::string scontrol_mode;
    static const std::string seffort("EFFORT"),svelocity("VELOCITY"),sposition("POSITION");
Joseph Mirabel's avatar
Joseph Mirabel committed
587
    static const std::string ros_control_mode = "ros_control_mode";
588
589
590
591

    /// Read the list of control_mode
    ros::NodeHandle rnh_ns("/sot_controller/control_mode/"+joint_name);
    
Joseph Mirabel's avatar
Joseph Mirabel committed
592
593
    ControlMode joint_control_mode;
    if (!rnh_ns.getParam(ros_control_mode,scontrol_mode))
Olivier Stasse's avatar
Olivier Stasse committed
594
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
595
596
597
598
599
        ROS_ERROR("No %s for %s - We found %s",
    	      ros_control_mode.c_str(),
    	      joint_name.c_str(),
    	      scontrol_mode.c_str());
        return false;
600
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
    
    if      (scontrol_mode==sposition)
      joint_control_mode=POSITION;
    else if (scontrol_mode==svelocity)
      joint_control_mode=VELOCITY;
    else if (scontrol_mode==seffort)
      joint_control_mode=EFFORT;
    else {
      ROS_ERROR("%s for %s not understood. Expected %s, %s or %s. Got %s",
    	    ros_control_mode.c_str(),
    	    joint_name.c_str(),
                sposition.c_str(),
                svelocity.c_str(),
                seffort.c_str(),
    	    scontrol_mode.c_str());
      return false;
    }
    
    aJointSotHandle.ros_control_mode = joint_control_mode;
    //aJointSotHandle.sot_control_mode = joint_control_mode;

622
623
    return true;
  }
624

625
626
627
628
629
630
631
632
633
634
  bool RCSotController::
  readParamsControlMode(ros::NodeHandle &robot_nh)
  {
    std::map<std::string,std::string> mapControlMode;
    
    // Read param from control_mode.
    if (robot_nh.hasParam("/sot_controller/control_mode")) 
      {
	/// For each listed joint
	for(unsigned int idJoint=0;idJoint<joints_name_.size();idJoint++)
Olivier Stasse's avatar
Olivier Stasse committed
635
	  {
636
637
638
639
	    std::string joint_name = joints_name_[idJoint];
	    JointSotHandle &aJoint = joints_[joint_name];
	    if (!getJointControlMode(joint_name,aJoint))
	      return false;
Joseph Mirabel's avatar
Joseph Mirabel committed
640
641
	    ROS_INFO("joint_name[%d]=%s, control_mode=%d",
                idJoint,joint_name.c_str(),aJoint.ros_control_mode);
Olivier Stasse's avatar
Olivier Stasse committed
642
643
	  }
      }
644
645
646
647
    else
      {
	ROS_INFO_STREAM("Default control mode : position");
      }
Olivier Stasse's avatar
Olivier Stasse committed
648
649
650
651
    /// Always return true;
    return true;
  }

652
653
654
  bool RCSotController::
  readParamsdt(ros::NodeHandle &robot_nh)
  {
655
656
657
658
    /// Reading the jitter is optional but it is a very good idea.
    if (robot_nh.hasParam("/sot_controller/jitter"))
      {
	robot_nh.getParam("/sot_controller/jitter",jitter_);
659
660
	if (verbosity_level_>0)
	  ROS_INFO_STREAM("jitter: " << jitter_);
661
662
      }

663
664
665
666
    /// Read /sot_controller/dt to know what is the control period
    if (robot_nh.hasParam("/sot_controller/dt"))
      {
	robot_nh.getParam("/sot_controller/dt",dt_);
667
668
	if (verbosity_level_>0)
	  ROS_INFO_STREAM("dt: " << dt_);
669
670
	return true;
      }
671

672
673
674
    ROS_ERROR("You need to define a control period in param /sot_controller/dt");
    return false;
  }
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690

  bool RCSotController::
  readUrdf(ros::NodeHandle &robot_nh)
  {
    /// Reading the parameter /robot_description which contains the robot
    /// description
    if (!robot_nh.hasParam("/robot_description"))
      {
	ROS_ERROR("ROS application does not have robot_description");
	return false;
      }
    std::string robot_description_str;
    
    robot_nh.getParam("/robot_description",robot_description_str);

    modelURDF_ = urdf::parseURDF(robot_description_str);
691
    if (verbosity_level_>0)
Olivier Stasse's avatar
Olivier Stasse committed
692
      ROS_INFO("Loaded /robot_description %ld",modelURDF_.use_count());
693
694
    return true;
  }
695
  
Olivier Stasse's avatar
Olivier Stasse committed
696
697
698
699
  bool RCSotController::
  readParams(ros::NodeHandle &robot_nh)
  {

700
701
702
703
704
    /// Read the level of verbosity for the controller (0: quiet, 1: info, 2: debug).
    /// Default to quiet
    readParamsVerbosityLevel(robot_nh);
    
    /// Reads the SoT dynamic library name.
Olivier Stasse's avatar
Olivier Stasse committed
705
706
707
    if (!readParamsSotLibName(robot_nh))
      return false;

708
709
    /// Read /sot_controller/simulation_mode to know if we are in simulation mode
    // Defines if we are in simulation node.
Olivier Stasse's avatar
Olivier Stasse committed
710
711
712
    if (robot_nh.hasParam("/sot_controller/simulation_mode")) 
      simulation_mode_ = true;
    
713
714
715
    /// Read URDF file.
    readUrdf(robot_nh);
    
716
717
    /// Calls readParamsJointNames
    // Reads the list of joints to be controlled.
Olivier Stasse's avatar
Olivier Stasse committed
718
719
720
    if (!readParamsJointNames(robot_nh))
      return false;

721
722
    /// Calls readParamsControlMode.
    // Defines if the control mode is position or effort
723
724
    if (!readParamsControlMode(robot_nh))
      return false;
Olivier Stasse's avatar
Olivier Stasse committed
725

726
727
    /// Calls readParamsFromRCToSotDevice
    // Mapping from ros-controll to sot device
Olivier Stasse's avatar
Olivier Stasse committed
728
    readParamsFromRCToSotDevice(robot_nh);
729

730
    /// Get control period
731
732
733
    if (!readParamsdt(robot_nh))
      return false;
    
734
    readParamsEffortControlPDMotorControlData(robot_nh);
Joseph Mirabel's avatar
Joseph Mirabel committed
735
    readParamsVelocityControlPDMotorControlData(robot_nh);
736
    readParamsPositionControlData(robot_nh);
Olivier Stasse's avatar
Olivier Stasse committed
737
738
739
740
741
742
743
744
    return true;
  }

    
  bool RCSotController::
  initJoints()
  {
    // Init Joint Names.
745
746
747
    //    joints_.resize(joints_name_.size());


Olivier Stasse's avatar
Olivier Stasse committed
748
749
    for (unsigned int i=0;i<nbDofs_;i++)
      {
750
	bool notok=true;
Joseph Mirabel's avatar
Joseph Mirabel committed
751

752
	while (notok)
Olivier Stasse's avatar
Olivier Stasse committed
753
	  {
754
	    std::string &joint_name = joints_name_[i];
755
756
	    try 
	      {
757
		JointSotHandle &aJointSotHandle = joints_[joint_name];
Joseph Mirabel's avatar
Joseph Mirabel committed
758
759
760
                switch (aJointSotHandle.ros_control_mode)
                {
                  case POSITION:
761
		    aJointSotHandle.joint = pos_iface_->getHandle(joint_name);
762
		    if (verbosity_level_>0)
763
764
		      ROS_INFO_STREAM("Found joint " << joint_name << " in position "
				      << i << " " << aJointSotHandle.joint.getName());
Joseph Mirabel's avatar
Joseph Mirabel committed
765
766
767
                    break;
                  case VELOCITY:
		    aJointSotHandle.joint = vel_iface_->getHandle(joint_name);
768
		    if (verbosity_level_>0)
Joseph Mirabel's avatar
Joseph Mirabel committed
769
		      ROS_INFO_STREAM("Found joint " << joint_name << " in velocity "
770
				      << i << " " << aJointSotHandle.joint.getName());
Joseph Mirabel's avatar
Joseph Mirabel committed
771
772
773
774
775
776
777
                    break;
                  case EFFORT:
                    aJointSotHandle.joint = effort_iface_->getHandle(joint_name);
                    if (verbosity_level_>0)
                      ROS_INFO_STREAM("Found joint " << joint_name << " in effort "
                          << i << " " << aJointSotHandle.joint.getName());
                }
778
779
780

		// throws on failure
		notok=false;
781
782
783
		aJointSotHandle.desired_init_pose =
		  aJointSotHandle.joint.getPosition();

784
785
786
787
	      }
	    catch (...)
	      {
		ROS_ERROR_STREAM("Could not find joint " 
788
789
				 << joint_name);
		return false;
790
	      }
Olivier Stasse's avatar
Olivier Stasse committed
791
792
793
794
795
796
797
798
799
800
	  }
      }
        
    return true ;
    
  }
  
  bool RCSotController::
  initIMU()
  {
801
802
    if (!imu_iface_) return false;

Olivier Stasse's avatar
Olivier Stasse committed
803
804
    // get all imu sensor names
    const std :: vector<std :: string >& imu_iface_names = imu_iface_->getNames();
805
806
807
808
809
    if (verbosity_level_>0)
      {
	for (unsigned i=0; i <imu_iface_names.size(); i++)
	  ROS_INFO("Got sensor %s", imu_iface_names[i].c_str());
      }
Olivier Stasse's avatar
Olivier Stasse committed
810
811
812
813
814
815
816
817
818
819
820
    for (unsigned i=0; i <imu_iface_names.size(); i++){
      // sensor handle on imu
      imu_sensor_.push_back(imu_iface_->getHandle(imu_iface_names[i]));
    }
 
    return true ;
  }
  
  bool RCSotController::
  initForceSensors()
  {
821
822
    if (!ft_iface_) return false;

Olivier Stasse's avatar
Olivier Stasse committed
823
824
    // get force torque sensors names package.
    const std::vector<std::string>& ft_iface_names = ft_iface_->getNames();
825
826
827
828
829
    if (verbosity_level_>0)
      {
	for (unsigned i=0; i <ft_iface_names.size(); i++)
	  ROS_INFO("Got sensor %s", ft_iface_names[i].c_str());
      }
Olivier Stasse's avatar
Olivier Stasse committed
830
831
832
833
    for (unsigned i=0; i <ft_iface_names.size(); i++){
      // sensor handle on torque forces
      ft_sensors_.push_back(ft_iface_->getHandle(ft_iface_names[i]));
    }
834
    profileLog_.nbForceSensors = ft_iface_names.size();
Olivier Stasse's avatar
Olivier Stasse committed
835
836
837
    return true;
  }

838
839
840
841
842
  bool RCSotController::
  initTemperatureSensors()
  {
    if (!simulation_mode_)
      {
843
#ifdef TEMPERATURE_SENSOR_CONTROLLER
844
845
        if (!act_temp_iface_) return false;

846
847
848
	// get temperature sensors names
	const std::vector<std::string>& act_temp_iface_names = act_temp_iface_->getNames();
	
849
850
851
852
853
854
855
856
	if (verbosity_level_>0)
	  {
	    ROS_INFO("Actuator temperature sensors: %ld",act_temp_iface_names.size() ); 
	    
	    for (unsigned i=0; i <act_temp_iface_names.size(); i++)
	      ROS_INFO("Got sensor %s", act_temp_iface_names[i].c_str());
	  }
	
857
858
859
860
	for (unsigned i=0; i <act_temp_iface_names.size(); i++){
	  // sensor handle on actuator temperature
	  act_temp_sensors_.push_back(act_temp_iface_->getHandle(act_temp_iface_names[i]));
	}
861
#endif	
862
863
864
865
      }

    return true;
  }
Olivier Stasse's avatar
Olivier Stasse committed
866
867
868
869
  
  void RCSotController::
  fillSensorsIn(std::string &title, std::vector<double> & data)
  {
870
871
    /// Tries to find the mapping from the local validation
    /// to the SoT device.
872
    it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice_.find(title);
873
    /// If the mapping is found
874
    if (it_mapRC2Sot!=mapFromRCToSotDevice_.end())
Olivier Stasse's avatar
Olivier Stasse committed
875
      {
876
	/// Expose the data to the SoT device.
Olivier Stasse's avatar
Olivier Stasse committed
877
878
879
880
881
882
883
884
885
	std::string lmapRC2Sot = it_mapRC2Sot->second;
	sensorsIn_[lmapRC2Sot].setName(lmapRC2Sot);
	sensorsIn_[lmapRC2Sot].setValues(data);
      }
  }

  void RCSotController::
  fillJoints()
  {
886
    
Olivier Stasse's avatar
Olivier Stasse committed
887
    /// Fill positions, velocities and torques.
888
889
890
    for(unsigned int idJoint=0;
	idJoint<joints_name_.size();
	idJoint++)
Olivier Stasse's avatar
Olivier Stasse committed
891
      {
892
893
894
895
896
897
	it_joint_sot_h anItJoint = joints_.find(joints_name_[idJoint]);
	if (anItJoint!=joints_.end())
	  {
	    JointSotHandle & aJoint = anItJoint->second;
	    DataOneIter_.motor_angle[idJoint] = aJoint.joint.getPosition();
	    
898
#ifdef TEMPERATURE_SENSOR_CONTROLLER
899
	    DataOneIter_.joint_angle[idJoint] = aJoint.joint.getAbsolutePosition();
900
#endif	  
901
	    DataOneIter_.velocities[idJoint] = aJoint.joint.getVelocity();
902

903
#ifdef TEMPERATURE_SENSOR_CONTROLLER	
904
	    DataOneIter_.torques[idJoint] = aJoint.joint.getTorqueSensor();
905
#endif
906
907
	    DataOneIter_.motor_currents[idJoint] = aJoint.joint.getEffort();
	  }
Olivier Stasse's avatar
Olivier Stasse committed
908
909
910
911
912
913
914
915
916
917
918
      }
    
    /// Update SoT internal values
    std::string ltitle("motor-angles"); 
    fillSensorsIn(ltitle,DataOneIter_.motor_angle);
    ltitle = "joint-angles";
    fillSensorsIn(ltitle,DataOneIter_.joint_angle);
    ltitle = "velocities";
    fillSensorsIn(ltitle,DataOneIter_.velocities);
    ltitle = "torques";
    fillSensorsIn(ltitle,DataOneIter_.torques);
919
920
    ltitle = "currents";
    fillSensorsIn(ltitle,DataOneIter_.motor_currents);
Olivier Stasse's avatar
Olivier Stasse committed
921
922
923
924
925
926
927
    
  }

  void RCSotController::setSensorsImu(std::string &name,
					   int IMUnb,
					   std::vector<double> & data)
  {
928
929
    std::ostringstream labelOss;
    labelOss << name << IMUnb;
Olivier Stasse's avatar
Olivier Stasse committed
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
    std::string label_s = labelOss.str();
    fillSensorsIn(label_s,data);
  }

  void RCSotController::
  fillImu()
  {
    for(unsigned int idIMU=0;idIMU<imu_sensor_.size();idIMU++)
      {
	/// Fill orientations, gyrometer and acceleration from IMU.
	if (imu_sensor_[idIMU].getOrientation())
	  {
	    for(unsigned int idquat = 0;idquat<4;idquat++)
	      {
		DataOneIter_.orientation[idquat] = imu_sensor_[idIMU].getOrientation ()[idquat];
	      }
	  }
	if (imu_sensor_[idIMU].getAngularVelocity())
	  {
	    for(unsigned int idgyrometer = 0;idgyrometer<3;
		idgyrometer++)
	      {
		DataOneIter_.gyrometer[idgyrometer] = 
		  imu_sensor_[idIMU].getAngularVelocity()[idgyrometer];
	      }
	  }
	if (imu_sensor_[idIMU].getLinearAcceleration())
	  {
	    for(unsigned int idlinacc = 0;idlinacc<3;
		idlinacc++)
	      {
		DataOneIter_.accelerometer[idlinacc] = 
		  imu_sensor_[idIMU].getLinearAcceleration()[idlinacc];
	      }
	  }
	
	std::string orientation_s("orientation_");
	setSensorsImu(orientation_s, idIMU, DataOneIter_.orientation);

	std::string gyrometer_s("gyrometer_");
	setSensorsImu(gyrometer_s, idIMU, DataOneIter_.gyrometer);

	std::string accelerometer_s("accelerometer_");
	setSensorsImu(accelerometer_s, idIMU, DataOneIter_.accelerometer);
      }
  }
  
  void RCSotController::
  fillForceSensors()
  {
    for(unsigned int idFS=0;idFS<ft_sensors_.size();
	idFS++)
      {
	for(unsigned int idForce=0;idForce<3;idForce++)
	  DataOneIter_.force_sensors[idFS*6+idForce]=
	    ft_sensors_[idFS].getForce()[idForce];
	for(unsigned int idTorque=0;idTorque<3;idTorque++)
	  DataOneIter_.force_sensors[idFS*6+3+idTorque]=
	    ft_sensors_[idFS].getTorque()[idTorque];
      }

    
    std::string alabel("forces");
    fillSensorsIn(alabel,DataOneIter_.force_sensors);
  }

996
997
998
999
1000
  void RCSotController::
  fillTempSensors()
  {
    if (!simulation_mode_)
      {
1001
#ifdef TEMPERATURE_SENSOR_CONTROLLER
1002
1003
1004
1005
	for(unsigned int idFS=0;idFS<act_temp_sensors_.size();idFS++)
	  {
	    DataOneIter_.temperatures[idFS]=  act_temp_sensors_[idFS].getValue();
	  }
1006
#endif
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
      }
    else
      {
	for(unsigned int idFS=0;idFS<nbDofs_;idFS++)
	  DataOneIter_.temperatures[idFS]=  0.0;
      }

    std::string alabel("act-temp");
    fillSensorsIn(alabel,DataOneIter_.temperatures);
  }
Olivier Stasse's avatar
Olivier Stasse committed
1017
1018
1019
1020
1021
1022
1023

  void RCSotController::
  fillSensors()
  {
    fillJoints();
    fillImu();
    fillForceSensors();
1024
    fillTempSensors();
Olivier Stasse's avatar
Olivier Stasse committed
1025
1026
1027
1028
1029
  }
  
  void RCSotController::
  readControl(std::map<std::string,dgs::ControlValues> &controlValues)
  {
1030
    ODEBUG4("joints_.size() = " << joints_.size());
1031
    std::string cmdTitle="control";
Olivier Stasse's avatar
Olivier Stasse committed
1032

1033
1034
    it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice_.find(cmdTitle);
    if (it_mapRC2Sot!=mapFromRCToSotDevice_.end())
Olivier Stasse's avatar
Olivier Stasse committed
1035
      {
1036
	std::string &lmapRC2Sot = it_mapRC2Sot->second;
Olivier Stasse's avatar
Olivier Stasse committed
1037
	command_ = controlValues[lmapRC2Sot].getValues();
1038
	ODEBUG4("angleControl_.size() = " << command_.size());
Olivier Stasse's avatar
Olivier Stasse committed
1039
1040
1041
	for(unsigned int i=0;
	    i<command_.size();++i)
	  {
1042
	    joints_[joints_name_[i]].joint.setCommand(command_[i]);
Joseph Mirabel's avatar
Joseph Mirabel committed
1043
1044
1045
            DataOneIter_.controls[i] = command_[i];
          }

Olivier Stasse's avatar
Olivier Stasse committed
1046
      }
1047
1048
    else
      ROS_INFO_STREAM("no control.");
Olivier Stasse's avatar
Olivier Stasse committed
1049
1050
1051
1052
  }

  void RCSotController::one_iteration()
  {
1053
    // Chrono start
1054
    RcSotLog_.start_it();
Olivier Stasse's avatar
Olivier Stasse committed
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
    
    /// Update the sensors.
    fillSensors();

    /// Generate a control law.
    try
      {
	sotController_->nominalSetSensors(sensorsIn_);
	sotController_->getControl(controlValues_);
      }
    catch(std::exception &e) { throw e;}

    /// Read the control values
    readControl(controlValues_);
1069
1070
    
    // Chrono stop.
1071
    RcSotLog_.stop_it();
1072
    
Olivier Stasse's avatar
Olivier Stasse committed
1073
    /// Store everything in Log.
1074
    RcSotLog_.record(DataOneIter_);
Olivier Stasse's avatar
Olivier Stasse committed
1075
1076
  }

1077
1078
1079
1080
1081
1082
1083
  void RCSotController::
  localStandbyEffortControlMode(const ros::Duration& period)
  {
    // ROS_INFO("Compute command for effort mode: %d %d",joints_.size(),effort_mode_pd_motors_.size());
    for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
      {
	std::string joint_name = joints_name_[idJoint];
1084
	std::map<std::string,ControlPDMotorControlData>::iterator
1085
1086
1087
1088
	  search_ecpd = effort_mode_pd_motors_.find(joint_name);
             
	if (search_ecpd!=effort_mode_pd_motors_.end())
	  {
1089
1090
1091
1092
	    ControlPDMotorControlData & ecpdcdata = search_ecpd->second;
	    JointSotHandle &aJointSotHandle = joints_[joint_name];
	    lhi::JointHandle &aJoint = aJointSotHandle.joint;

1093
	    double vel_err = 0 - aJoint.getVelocity();
1094
	    double err = aJointSotHandle.desired_init_pose - aJoint.getPosition();
1095
1096
1097
	    
	    double local_command = ecpdcdata.pid_controller.computeCommand(err,vel_err,period);
	    // Apply command
1098
	    aJoint.setCommand(local_command);
1099
1100
1101
	  }
      }
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139

  void RCSotController::
  localStandbyVelocityControlMode(const ros::Duration& period)
  {
    static bool first_time=true;
    
    /// Iterate over all the joints
    for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
      {
        /// Find the joint
        std::string joint_name = joints_name_[idJoint];
	std::map<std::string,ControlPDMotorControlData>::iterator
	  search_ecpd = velocity_mode_pd_motors_.find(joint_name);
             
	if (search_ecpd!=velocity_mode_pd_motors_.end())
	  {
	    ControlPDMotorControlData & ecpdcdata = search_ecpd->second;
	    JointSotHandle &aJointSotHandle = joints_[joint_name];
	    lhi::JointHandle &aJoint = aJointSotHandle.joint;

	    double vel_err = 0 - aJoint.getVelocity();
	    double err = aJointSotHandle.desired_init_pose - aJoint.getPosition();
	    
	    double local_command = ecpdcdata.pid_controller.computeCommand(err,vel_err,period);
		    
	    aJoint.setCommand(local_command);
	    
	    assert(aJoint.getName() == joint_name);
	    if (first_time)
	      if (verbosity_level_>1) {
		ROS_INFO("Control joint %s (id %d) to %f\n",
			 joint_name.c_str(),idJoint,
			 aJoint.getPosition());
	      }
	  }
      }
    first_time=false;
  }
1140
  
1141
1142
1143
  void RCSotController::
  localStandbyPositionControlMode()
  {
1144
1145
    static bool first_time=true;
    
1146
1147
1148
    /// Iterate over all the joints
    for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
      {
1149
1150
        /// Find the joint
        std::string joint_name = joints_name_[idJoint];
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
	
	// If it is position mode control.
	if (joints_[joint_name].ros_control_mode==POSITION)
	  {	    
	    JointSotHandle &aJointSotHandle = joints_[joint_name];
	    lhi::JointHandle &aJoint = aJointSotHandle.joint;
		    
	    aJoint.setCommand(aJointSotHandle.desired_init_pose);
	    
	    assert(aJoint.getName() == joint_name);
	    if (first_time)
	      if (verbosity_level_>1) {
		ROS_INFO("Control joint %s (id %d) to %f\n",
			 joint_name.c_str(),idJoint,
			 aJoint.getPosition());
	      }
	  }
1168
      }
1169
    first_time=false;
1170
1171
  }
  
Olivier Stasse's avatar
Olivier Stasse committed
1172
  void RCSotController::
1173
1174
  update(const ros::Time&, const ros::Duration& period)
   {
1175
     // Do not send any control if the dynamic graph is not started
1176
     if (!isDynamicGraphStopped())
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
       {
	 try
	   {
	     double periodInSec = period.toSec();
	     if (periodInSec+accumulated_time_>dt_-jitter_)
	       {
		 one_iteration();
		 accumulated_time_ = 0.0;
	       }
	     else
	       accumulated_time_ += periodInSec;
	   }
	 catch (std::exception const &exc)
	   {
	     std::cerr << "Failure happened during one_iteration evaluation: std_exception" << std::endl;
	     std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
	     std::cerr << __FILE__ << " " << __LINE__  << std::endl;
	     throw exc;
	   }
	 catch (...)
	   {
	     std::cerr << "Failure happened during one_iteration evaluation: unknown exception" << std::endl;
	     std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
	     std::cerr << __FILE__ << " " << __LINE__  << std::endl;
	   }
       }
     else
       {
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
         /// Update the sensors.
         fillSensors();

         /// Generate a control law.
         try
         {
           sotController_->nominalSetSensors(sensorsIn_);
         }
         catch(std::exception &e) { throw e;}

1215
1216
1217
1218
	 // But in effort mode it means that we are sending 0
	 // Therefore implements a default PD controller on the system.
	 // Applying both to handle mixed system.
	 localStandbyEffortControlMode(period);
1219
         localStandbyVelocityControlMode(period);
1220
1221
	 localStandbyPositionControlMode();
       }
1222
   }
Olivier Stasse's avatar
Olivier Stasse committed
1223
1224
1225
1226
  
  void RCSotController::
  starting(const ros::Time &)
  {
1227
1228
1229
    using namespace ::dynamicgraph;
    RealTimeLogger::instance().addOutputStream(LoggerStreamPtr_t(new LoggerROSStream()));

Olivier Stasse's avatar
Olivier Stasse committed
1230
1231
1232
1233
1234
1235
    fillSensors();
  }
    
  void RCSotController::
  stopping(const ros::Time &)
  {
1236
    std::string afilename("/tmp/sot.log");
1237
    RcSotLog_.save(afilename);
1238
1239

    SotLoaderBasic::CleanUp();
1240
1241
1242

    using namespace ::dynamicgraph;
    RealTimeLogger::destroy();
Olivier Stasse's avatar
Olivier Stasse committed
1243
1244
1245
  }
  

1246
  PLUGINLIB_EXPORT_CLASS(sot_controller::RCSotController, 
1247
			 lci::ControllerBase)
Olivier Stasse's avatar
Olivier Stasse committed
1248
}