GraspController.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "GraspController.h"
11 #include "hrpsys/util/VectorConvert.h"
12 #include <rtm/CorbaNaming.h>
13 #include <hrpModel/ModelLoaderUtil.h>
14 #include "hrpsys/idl/RobotHardwareService.hh"
15 
16 #include <hrpModel/Link.h>
17 
18 #include <math.h>
19 #define deg2rad(x)((x)*M_PI/180)
20 #define min(a,b) ((a)<(b)?(a):(b))
21 #define max(a,b) ((a)>(b)?(a):(b))
22 
23 // Module specification
24 // <rtc-template block="module_spec">
25 static const char* softerrorlimiter_spec[] =
26  {
27  "implementation_id", "GraspController",
28  "type_name", "GraspController",
29  "description", "soft error limiter",
30  "version", HRPSYS_PACKAGE_VERSION,
31  "vendor", "AIST",
32  "category", "example",
33  "activity_type", "DataFlowComponent",
34  "max_instance", "10",
35  "language", "C++",
36  "lang_type", "compile",
37  // Configuration variables
38  "conf.default.debugLevel", "0",
39  ""
40  };
41 // </rtc-template>
42 
44  : RTC::DataFlowComponentBase(manager),
45  // <rtc-template block="initializer">
46  m_qRefIn("qRef", m_qRef),
47  m_qCurrentIn("qCurrent", m_qCurrent),
48  m_qIn("qIn", m_q),
49  m_qOut("q", m_q),
50  m_GraspControllerServicePort("GraspControllerService"),
51  // </rtc-template>
52  m_debugLevel(0),
53  dummy(0)
54 {
55  m_service0.grasp(this);
56 }
57 
59 {
60 }
61 
62 
63 
64 RTC::ReturnCode_t GraspController::onInitialize()
65 {
66  std::cout << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl;
67  // <rtc-template block="bind_config">
68  // Bind variables and configuration variable
69  bindParameter("debugLevel", m_debugLevel, "0");
70 
71  // </rtc-template>
72 
73  // Registration: InPort/OutPort/Service
74  // <rtc-template block="registration">
75  // Set InPort buffers
76  addInPort("qRef", m_qRefIn); // for naming rule of hrpsys_config.py
77  addInPort("qCurrent", m_qCurrentIn);
78  addInPort("qIn", m_qIn);
79 
80  // Set OutPort buffer
81  addOutPort("q", m_qOut); // for naming rule of hrpsys_config.py
82 
83  // Set service provider to Ports
84  m_GraspControllerServicePort.registerProvider("service0", "GraspControllerService", m_service0);
85 
86  // Set service consumers to Ports
87 
88  // Set CORBA Service Ports
90 
91  // </rtc-template>
92 
95  coil::stringTo(m_dt, prop["dt"].c_str());
96 
97  RTC::Manager& rtcManager = RTC::Manager::instance();
98  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
99  int comPos = nameServer.find(",");
100  if (comPos < 0){
101  comPos = nameServer.length();
102  }
103  nameServer = nameServer.substr(0, comPos);
104  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
105  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
106  CosNaming::NamingContext::_duplicate(naming.getRootContext())
107  )){
108  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
109  << std::endl;
110  return RTC::RTC_ERROR;
111  }
112  // <name> : <joint1>, <direction1>, <joint2>, <direction2>, <joint1>, <direction2>, <name> : <joint1>, <direction1>
113  // check num of grasp
114  coil::vstring grasp_joint_params = coil::split(prop["grasp_joint_groups"], ",");
115  std::string grasp_name;
116  GraspJoint grasp_joint;
117  std::vector<GraspJoint> grasp_joints;
118  std::cerr << "[" << m_profile.instance_name << "] Parse joint group setting..." << std::endl;
119  for(unsigned int i = 0, f = 0; i < grasp_joint_params.size(); i++ ){
120  coil::vstring grasp_joint_group_names = coil::split(grasp_joint_params[i], ":");
121  if ( grasp_joint_group_names.size() > 1 ) {
122  if ( grasp_name != "" ) {
123  GraspParam grasp_param;
124  grasp_param.time = 0;
125  grasp_param.joints = grasp_joints;
126  grasp_param.time = 1; // stop
127  m_grasp_param[grasp_name] = grasp_param;
128  grasp_joints.clear();
129  }
130  // initialize
131  grasp_name = grasp_joint_group_names[0];
132  if ( !! m_robot->link(grasp_joint_group_names[1]) ) {
133  grasp_joint.id = m_robot->link(std::string(grasp_joint_group_names[1].c_str()))->jointId;
134  } else {
135  std::cerr << "[" << m_profile.instance_name << "] No such grasp joint name " << grasp_joint_group_names[1] << std::endl;
136  }
137  f = 0;
138  i++;
139  } else {
140  std::cerr << "[" << m_profile.instance_name << "] Invalid joint group setting (length " << grasp_joint_group_names.size() << " should be > 1" << std::endl;
141  }
142  if ( f == 0 ) {
143  coil::stringTo(grasp_joint.dir,grasp_joint_params[i].c_str());
144  grasp_joints.push_back(grasp_joint);
145  f = 1 ;
146  } else {
147  if ( !! m_robot->link(grasp_joint_params[i]) ) {
148  grasp_joint.id = m_robot->link(grasp_joint_params[i])->jointId;
149  } else {
150  std::cerr << "[" << m_profile.instance_name << "] No such grasp joint name " << grasp_joint_params[i] << std::endl;
151  }
152  f = 0 ;
153  }
154  }
155  // finalize
156  if ( grasp_name != "" ) {
157  GraspParam grasp_param;
158  grasp_param.time = 0;
159  grasp_param.joints = grasp_joints;
160  grasp_param.time = 1; // stop
161  m_grasp_param[grasp_name] = grasp_param;
162  }
163  //
164  std::cerr << "[" << m_profile.instance_name << "] Joint group setting results." << std::endl;
165  std::map<std::string, GraspParam >::iterator it = m_grasp_param.begin();
166  while ( it != m_grasp_param.end() ) {
167  std::cerr << "[" << m_profile.instance_name << "] " << it->first << " : ";
168  for ( unsigned int i = 0 ; i < it->second.joints.size(); i++ ) {
169  std::cerr << "id = " << it->second.joints[i].id << ", dir = " << it->second.joints[i].dir << ", ";
170  }
171  std::cerr << std::endl;
172  it++;
173  }
174 
175 
176  return RTC::RTC_OK;
177 }
178 
179 /*
180 RTC::ReturnCode_t GraspController::onFinalize()
181 {
182  return RTC::RTC_OK;
183 }
184 */
185 
186 /*
187 RTC::ReturnCode_t GraspController::onStartup(RTC::UniqueId ec_id)
188 {
189  return RTC::RTC_OK;
190 }
191 */
192 
193 /*
194 RTC::ReturnCode_t GraspController::onShutdown(RTC::UniqueId ec_id)
195 {
196  return RTC::RTC_OK;
197 }
198 */
199 
201 {
202  std::cout << "[" << m_profile.instance_name<< "] : onActivated(" << ec_id << ")" << std::endl;
203  return RTC::RTC_OK;
204 }
205 
207 {
208  std::cout << "[" << m_profile.instance_name<< "] : onDeactivated(" << ec_id << ")" << std::endl;
209  for (std::map<std::string, GraspParam >::iterator it = m_grasp_param.begin(); it != m_grasp_param.end(); it++ ) {
210  it->second.time = 2; // count down to 1
211  it->second.target_error = 0;
212  }
213  return RTC::RTC_OK;
214 }
215 
217 {
218  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
219  if (m_qRefIn.isNew()) {
220  m_qRefIn.read();
221  }
222  if (m_qCurrentIn.isNew()) {
223  m_qCurrentIn.read();
224  }
225  if (m_qIn.isNew()) {
226  m_qIn.read();
227  }
228 
229  if ( m_qRef.data.length() == m_qCurrent.data.length() &&
230  m_qRef.data.length() == m_q.data.length() ) {
231 
232  std::map<std::string, GraspParam >::iterator it = m_grasp_param.begin();
233  while ( it != m_grasp_param.end() ) {
234  GraspParam& grasp_param = it->second;
235  if ( grasp_param.time < 0 ) { // staring
236  grasp_param.time++;
237  } else if ( grasp_param.time == 0 ) {// working
238  //std::cerr << "grasp mode " << std::endl;
239  for ( unsigned int j= 0; j < grasp_param.joints.size(); j++ ) {
240  int i = grasp_param.joints[j].id;
241  if ( 0 <= i && (unsigned int)i < m_qRef.data.length() ) {
242  double error = (m_qCurrent.data[i] - m_qRef.data[i]) + grasp_param.target_error * grasp_param.joints[j].dir;
243  double diff = fabs(error);
244  if ( error > 0 ) m_q.data[i] = m_qRef.data[i] + diff;
245  if ( error < 0 ) m_q.data[i] = m_qRef.data[i] - diff;
246  //std::cerr << "id = " << i << ", ref = " << m_qRef.data[i] << ", cur =" << m_qCurrent.data[i] << " error = " << error << ", diff = " << diff << ", q = " << m_q.data[i] << " (target = " << grasp_param.target_error << ", dir=" << grasp_param.joints[j].dir << ")" << std::endl;
247  } else {
248  if (m_debugLevel==1) std::cerr << "GraspController is not working..., id = " << i << std::endl;
249  }
250  }
251  } else if ( grasp_param.time > 1 ) { // stopping
252  grasp_param.time--;
253  for ( unsigned int j= 0; j < grasp_param.joints.size(); j++ ) {
254  int i = grasp_param.joints[j].id;
255  if ( 0 <= i && (unsigned int)i < m_qRef.data.length() ) {
256  m_qRef.data[i] = (m_qRef.data[i] - m_q.data[i] ) * grasp_param.time/1000 + m_q.data[i];
257  double diff = m_qRef.data[i] - m_qCurrent.data[i];
258  if ( diff > 0 ) diff = min(diff, 0.034907); // 2 [deg]
259  if ( diff < 0 ) diff = max(diff,-0.034907); // 2 [deg]
260  m_q.data[i] = diff + m_qCurrent.data[i];
261  //std::cerr << "id = " << i << ", ref = " << m_qRef.data[i] << ", cur =" << m_qCurrent.data[i] << " q = " << m_q.data[i] << std::endl;
262  }
263  }
264  } else if ( grasp_param.time == 1 ) {// stop
265  }
266  it++;
267  }
268 
269  m_qOut.write();
270  }else if ( m_qCurrent.data.length() == m_q.data.length() ) {
271  if (m_debugLevel==1) std::cerr << "GraspController in pass through mode..." << std::endl;
272  m_qOut.write();
273  } else {
274  std::cerr << "GraspController is not working..." << std::endl;
275  std::cerr << " m_qIn " << m_q.data.length() << std::endl;
276  std::cerr << " m_qRef " << m_qRef.data.length() << std::endl;
277  std::cerr << " m_qCurrent " << m_qCurrent.data.length() << std::endl;
278  }
279 
280  return RTC::RTC_OK;
281 }
282 
283 /*
284 RTC::ReturnCode_t GraspController::onAborting(RTC::UniqueId ec_id)
285 {
286  return RTC::RTC_OK;
287 }
288 */
289 
290 /*
291 RTC::ReturnCode_t GraspController::onError(RTC::UniqueId ec_id)
292 {
293  return RTC::RTC_OK;
294 }
295 */
296 
297 /*
298 RTC::ReturnCode_t GraspController::onReset(RTC::UniqueId ec_id)
299 {
300  return RTC::RTC_OK;
301 }
302 */
303 
304 /*
305 RTC::ReturnCode_t GraspController::onStateUpdate(RTC::UniqueId ec_id)
306 {
307  return RTC::RTC_OK;
308 }
309 */
310 
311 /*
312 RTC::ReturnCode_t GraspController::onRateChanged(RTC::UniqueId ec_id)
313 {
314  return RTC::RTC_OK;
315 }
316 */
317 
318  // m_grasp_param.time
319  // > 1 : stopping
320  // 1 : stopped
321  // 0 : working
322  // < 0 : starting
323 bool GraspController::startGrasp(const char *name, double target_error) {
324  if ( m_grasp_param.find( name ) == m_grasp_param.end() ) {
325  std::cerr << "[" << m_profile.instance_name << "] Could not found grasp controller " << name << std::endl;
326  return false;
327  }
328  std::cerr << "[" << m_profile.instance_name << "] Start Grasp " << name << std::endl;
329  m_grasp_param[name].time = -10; // count up to 0
330  m_grasp_param[name].target_error = fabs(target_error);
331  return true;
332 }
333 
334 bool GraspController::stopGrasp(const char *name) {
335  if ( m_grasp_param.find( name ) == m_grasp_param.end() ) {
336  std::cerr << "[" << m_profile.instance_name << "] Could not found grasp controller " << name << std::endl;
337  return false;
338  }
339  std::cerr << "[" << m_profile.instance_name << "] Stop Grasp " << name << std::endl;
340  m_grasp_param[name].time = 1000; // count down to 1
341  m_grasp_param[name].target_error = 0;
342  return true;
343 }
344 
345 
346 extern "C"
347 {
348 
350  {
352  manager->registerFactory(profile,
353  RTC::Create<GraspController>,
354  RTC::Delete<GraspController>);
355  }
356 
357 };
358 
359 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
#define max(a, b)
hrp::BodyPtr m_robot
std::vector< GraspJoint > joints
bool stringTo(To &val, const char *str)
InPort< TimedDoubleSeq > m_qIn
InPort< TimedDoubleSeq > m_qRefIn
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
void error(char *msg) const
void GraspControllerInit(RTC::Manager *manager)
CORBA::ORB_ptr getORB()
png_uint_32 i
coil::Properties & getProperties()
static Manager & instance()
bool addOutPort(const char *name, OutPortBase &outport)
boost::shared_ptr< Body > BodyPtr
#define min(a, b)
std::vector< std::string > vstring
coil::Properties & getConfig()
unsigned int m_debugLevel
ExecutionContextHandle_t UniqueId
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
const std::string & name()
null component
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
void grasp(GraspController *i_grasp)
OutPort< TimedDoubleSeq > m_qOut
static const char * softerrorlimiter_spec[]
virtual RTC::ReturnCode_t onInitialize()
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
prop
TimedDoubleSeq m_q
bool startGrasp(const char *name, double target_error)
naming
RTC::CorbaPort m_GraspControllerServicePort
TimedDoubleSeq m_qCurrent
virtual bool isNew()
virtual ~GraspController()
Destructor.
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
bool addPort(PortBase &port)
std::map< std::string, GraspParam > m_grasp_param
virtual bool write(DataType &value)
InPort< TimedDoubleSeq > m_qCurrentIn
bool addInPort(const char *name, InPortBase &inport)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
bool stopGrasp(const char *name)
GraspController(RTC::Manager *manager)
Constructor.
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)
TimedDoubleSeq m_qRef
GraspControllerService_impl m_service0


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20