ForwardKinematics.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <rtm/CorbaNaming.h>
11 #include "ForwardKinematics.h"
12 
13 #include "hrpModel/Link.h"
14 #include "hrpModel/ModelLoaderUtil.h"
15 
17 
18 // Module specification
19 // <rtc-template block="module_spec">
20 static const char* nullcomponent_spec[] =
21  {
22  "implementation_id", "ForwardKinematics",
23  "type_name", "ForwardKinematics",
24  "description", "forward kinematics component",
25  "version", HRPSYS_PACKAGE_VERSION,
26  "vendor", "AIST",
27  "category", "example",
28  "activity_type", "DataFlowComponent",
29  "max_instance", "10",
30  "language", "C++",
31  "lang_type", "compile",
32  // Configuration variables
33  "conf.default.sensorAttachedLink", "",
34 
35  ""
36  };
37 // </rtc-template>
38 
40  : RTC::DataFlowComponentBase(manager),
41  // <rtc-template block="initializer">
42  m_qIn("q", m_q),
43  m_sensorRpyIn("sensorRpy", m_sensorRpy),
44  m_qRefIn("qRef", m_qRef),
45  m_basePosRefIn("basePosRef", m_basePosRef),
46  m_baseRpyRefIn("baseRpyRef", m_baseRpyRef),
47  m_ForwardKinematicsServicePort("ForwardKinematicsService"),
48  // </rtc-template>
49  dummy(0)
50 {
51 }
52 
54 {
55 }
56 
57 
58 
60 {
61  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
62  // <rtc-template block="bind_config">
63  // Bind variables and configuration variable
65  bindParameter("sensorAttachedLink", m_sensorAttachedLinkName, ref["conf.default.sensorAttachedLink"].c_str());
66 
67  // </rtc-template>
68 
69  // Registration: InPort/OutPort/Service
70  // <rtc-template block="registration">
71  // Set InPort buffers
72  addInPort("q", m_qIn);
73  addInPort("sensorRpy", m_sensorRpyIn);
74  addInPort("qRef", m_qRefIn);
75  addInPort("basePosRef", m_basePosRefIn);
76  addInPort("baseRpyRef", m_baseRpyRefIn);
77 
78  // Set OutPort buffer
79 
80  // Set service provider to Ports
81  m_ForwardKinematicsServicePort.registerProvider("service0", "ForwardKinematicsService", m_service0);
83  m_service0.setComp(this);
84 
85  // Set service consumers to Ports
86 
87  // Set CORBA Service Ports
88 
89  // </rtc-template>
90 
92 
93  RTC::Manager& rtcManager = RTC::Manager::instance();
94  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
95  int comPos = nameServer.find(",");
96  if (comPos < 0){
97  comPos = nameServer.length();
98  }
99  nameServer = nameServer.substr(0, comPos);
100  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
102  if (!loadBodyFromModelLoader(m_refBody, prop["model"].c_str(),
103  CosNaming::NamingContext::_duplicate(naming.getRootContext()))){
104  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
105  return RTC::RTC_ERROR;
106  }
108  if (!loadBodyFromModelLoader(m_actBody, prop["model"].c_str(),
109  CosNaming::NamingContext::_duplicate(naming.getRootContext()))){
110  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
111  return RTC::RTC_ERROR;
112  }
113 
114  m_refLink = m_refBody->rootLink();
115  m_actLink = m_actBody->rootLink();
116 
117  return RTC::RTC_OK;
118 }
119 
120 
121 
122 /*
123 RTC::ReturnCode_t ForwardKinematics::onFinalize()
124 {
125  return RTC::RTC_OK;
126 }
127 */
128 
129 /*
130 RTC::ReturnCode_t ForwardKinematics::onStartup(RTC::UniqueId ec_id)
131 {
132  return RTC::RTC_OK;
133 }
134 */
135 
136 /*
137 RTC::ReturnCode_t ForwardKinematics::onShutdown(RTC::UniqueId ec_id)
138 {
139  return RTC::RTC_OK;
140 }
141 */
142 
144 {
145  std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
146  if (m_sensorAttachedLinkName == ""){
147  m_sensorAttachedLink = NULL;
148  }else{
150  if (!m_sensorAttachedLink){
151  std::cerr << "can't find a link named " << m_sensorAttachedLinkName
152  << std::endl;
153  return RTC::RTC_ERROR;
154  }
155  }
156  return RTC::RTC_OK;
157 }
158 
160 {
161  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
162  return RTC::RTC_OK;
163 }
164 
166 {
167  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
168 
170  m_tm.sec = tm.sec();
171  m_tm.nsec = tm.usec() * 1000;
172 
173  if (m_qIn.isNew()) {
174  m_qIn.read();
175  for (unsigned int i=0; i<m_actBody->numJoints(); i++){
176  m_actBody->joint(i)->q = m_q.data[i];
177  }
178  }
179 
180  if (m_sensorRpyIn.isNew()) {
182  hrp::Matrix33 sensorR = hrp::rotFromRpy(m_sensorRpy.data.r,
183  m_sensorRpy.data.p,
184  m_sensorRpy.data.y);
186  hrp::Matrix33 sensor2base(m_sensorAttachedLink->R.transpose()*m_actBody->rootLink()->R);
187  hrp::Matrix33 baseR(sensorR*sensor2base);
188  // to prevent numerical error
189  hrp::Vector3 baseRpy = hrp::rpyFromRot(baseR);
190  // use reference yaw angle instead of estimated one
191  baseRpy[2] = m_baseRpyRef.data.y;
192  m_actBody->rootLink()->R = hrp::rotFromRpy(baseRpy);
193  }else{
194  m_actBody->rootLink()->R = sensorR;
195  }
196  }
197 
198  if (m_qRefIn.isNew()) {
199  m_qRefIn.read();
200  for (unsigned int i=0; i<m_refBody->numJoints(); i++){
201  m_refBody->joint(i)->q = m_qRef.data[i];
202  }
203  }
204 
205  if (m_basePosRefIn.isNew()){
207  hrp::Link *root = m_refBody->rootLink();
208  root->p[0] = m_basePosRef.data.x;
209  root->p[1] = m_basePosRef.data.y;
210  root->p[2] = m_basePosRef.data.z;
211  }
212 
213  if (m_baseRpyRefIn.isNew()){
215  hrp::Vector3 rpy;
216  rpy[0] = m_baseRpyRef.data.r;
217  rpy[1] = m_baseRpyRef.data.p;
218  rpy[2] = m_baseRpyRef.data.y;
219  m_refBody->rootLink()->R = hrp::rotFromRpy(rpy);
220 
221  }
222 
223  {
224  Guard guard(m_bodyMutex);
225  m_refBody->calcForwardKinematics();
226  m_actBody->calcForwardKinematics();
227  }
228 
229  return RTC::RTC_OK;
230 }
231 
232 /*
233 RTC::ReturnCode_t ForwardKinematics::onAborting(RTC::UniqueId ec_id)
234 {
235  return RTC::RTC_OK;
236 }
237 */
238 
239 /*
240 RTC::ReturnCode_t ForwardKinematics::onError(RTC::UniqueId ec_id)
241 {
242  return RTC::RTC_OK;
243 }
244 */
245 
246 /*
247 RTC::ReturnCode_t ForwardKinematics::onReset(RTC::UniqueId ec_id)
248 {
249  return RTC::RTC_OK;
250 }
251 */
252 
253 /*
254 RTC::ReturnCode_t ForwardKinematics::onStateUpdate(RTC::UniqueId ec_id)
255 {
256  return RTC::RTC_OK;
257 }
258 */
259 
260 /*
261 RTC::ReturnCode_t ForwardKinematics::onRateChanged(RTC::UniqueId ec_id)
262 {
263  return RTC::RTC_OK;
264 }
265 */
266 
267 ::CORBA::Boolean ForwardKinematics::getReferencePose(const char* linkname, RTC::TimedDoubleSeq_out pose,const char* frame_name)
268 {
269  pose = new RTC::TimedDoubleSeq();
270  Guard guard(m_bodyMutex);
271  hrp::Link *l = m_refBody->link(linkname);
272  if (!l) return false;
273  hrp::Link *f = NULL;
274  if (frame_name) {
275  f = m_refBody->link(frame_name);
276  if (!f) {
277  std::cerr << "[getReferencePose] ERROR Could not find frame_name = " << frame_name << std::endl;
278  return false;
279  }
280  }
281  std::cerr << "[getReferencePose] linkaname = " << linkname << ", frame_name = " << (frame_name?frame_name:"(null)") << std::endl;
282  hrp::Vector3 p = l->p;
283  hrp::Matrix33 R = l->attitude();
284  if (!!f) {
285  p = f->attitude().transpose() * ( p - f->p );
286  R = f->attitude().transpose() * R;
287  }
288  pose->tm = m_tm;
289  pose->data.length(16);
290  pose->data[ 0]=R(0,0);pose->data[ 1]=R(0,1);pose->data[ 2]=R(0,2);pose->data[ 3]=p[0];
291  pose->data[ 4]=R(1,0);pose->data[ 5]=R(1,1);pose->data[ 6]=R(1,2);pose->data[ 7]=p[1];
292  pose->data[ 8]=R(2,0);pose->data[ 9]=R(2,1);pose->data[10]=R(2,2);pose->data[11]=p[2];
293  pose->data[12]=0; pose->data[13]=0; pose->data[14]=0; pose->data[15]=1;
294  return true;
295 }
296 
297 ::CORBA::Boolean ForwardKinematics::getCurrentPose(const char* linkname, RTC::TimedDoubleSeq_out pose, const char* frame_name)
298 {
299  pose = new RTC::TimedDoubleSeq();
300  Guard guard(m_bodyMutex);
301  hrp::Link *l = m_actBody->link(linkname);
302  if (!l) return false;
303  hrp::Link *f = NULL;
304  if (frame_name) {
305  f = m_actBody->link(frame_name);
306  if (!f) {
307  std::cerr << "[getCurrentPose] ERROR Could not find frame_name = " << frame_name << std::endl;
308  return false;
309  }
310  }
311  std::cerr << "[getCurrentPose] linkaname = " << linkname << ", frame_name = " << (frame_name?frame_name:"(null)") << std::endl;
313 
314  hrp::Vector3 p(l->p + dp);
315  hrp::Matrix33 R = l->attitude();
316  if (!!f) {
317  p = f->attitude().transpose() * ( p - f->p);
318  R = f->attitude().transpose() * R;
319  }
320  pose->tm = m_tm;
321  pose->data.length(16);
322  pose->data[ 0]=R(0,0);pose->data[ 1]=R(0,1);pose->data[ 2]=R(0,2);pose->data[ 3]=p[0];
323  pose->data[ 4]=R(1,0);pose->data[ 5]=R(1,1);pose->data[ 6]=R(1,2);pose->data[ 7]=p[1];
324  pose->data[ 8]=R(2,0);pose->data[ 9]=R(2,1);pose->data[10]=R(2,2);pose->data[11]=p[2];
325  pose->data[12]=0; pose->data[13]=0; pose->data[14]=0; pose->data[15]=1;
326  return true;
327 }
328 
329 ::CORBA::Boolean ForwardKinematics::getRelativeCurrentPosition(const char* linknameFrom, const char* linknameTo, const OpenHRP::ForwardKinematicsService::position target, OpenHRP::ForwardKinematicsService::position result)
330 {
331  Guard guard(m_bodyMutex);
332  hrp::Link *from = m_actBody->link(linknameFrom);
333  hrp::Link *to = m_actBody->link(linknameTo);
334  if (!from || !to) return false;
335  hrp::Vector3 targetPrel(target[0], target[1], target[2]);
336  hrp::Vector3 targetPabs(to->p+to->attitude()*targetPrel);
337  hrp::Matrix33 Rt(from->attitude().transpose());
338  hrp::Vector3 p(Rt*(targetPabs - from->p));
339  result[ 0]=p(0);result[ 1]=p(1);result[ 2]=p(2);
340  return true;
341 }
342 
343 ::CORBA::Boolean ForwardKinematics::selectBaseLink(const char* linkname)
344 {
345  Guard guard(m_bodyMutex);
346  hrp::Link *l = m_refBody->link(linkname);
347  if (!l) return false;
348  m_refLink = l;
349  m_actLink = m_actBody->link(linkname);
350  return true;
351 }
352 
353 extern "C"
354 {
355 
357  {
359  manager->registerFactory(profile,
360  RTC::Create<ForwardKinematics>,
361  RTC::Delete<ForwardKinematics>);
362  }
363 
364 };
365 
366 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
std::string m_sensorAttachedLinkName
null component
static const char * nullcomponent_spec[]
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
TimedOrientation3D m_baseRpyRef
InPort< TimedDoubleSeq > m_qIn
long int sec() const
coil::Guard< coil::Mutex > Guard
InPort< TimedPoint3D > m_basePosRefIn
void ForwardKinematicsInit(RTC::Manager *manager)
InPort< TimedOrientation3D > m_sensorRpyIn
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
CORBA::ORB_ptr getORB()
png_uint_32 i
ForwardKinematicsService_impl m_service0
coil::Properties & getProperties()
static Manager & instance()
boost::shared_ptr< Body > BodyPtr
hrp::BodyPtr m_actBody
Eigen::Vector3d Vector3
RTC::CorbaPort m_ForwardKinematicsServicePort
Matrix33 rotFromRpy(const Vector3 &rpy)
hrp::BodyPtr m_refBody
Eigen::Matrix3d Matrix33
int gettimeofday(struct timeval *tv, struct timezone *tz)
coil::Properties & getConfig()
TimedDoubleSeq m_q
::CORBA::Boolean getRelativeCurrentPosition(const char *linknameFrom, const char *linknameTo, const OpenHRP::ForwardKinematicsService::position target, OpenHRP::ForwardKinematicsService::position result)
ExecutionContextHandle_t UniqueId
::CORBA::Boolean getReferencePose(const char *linkname, RTC::TimedDoubleSeq_out pose, const char *frame_name)
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
::CORBA::Boolean getCurrentPose(const char *linkname, RTC::TimedDoubleSeq_out pose, const char *frame_name)
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
InPort< TimedDoubleSeq > m_qRefIn
hrp::Link * m_sensorAttachedLink
void setComp(ForwardKinematics *i_comp)
prop
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
naming
virtual RTC::ReturnCode_t onInitialize()
InPort< TimedOrientation3D > m_baseRpyRefIn
virtual bool isNew()
ForwardKinematics(RTC::Manager *manager)
Constructor.
TimedPoint3D m_basePosRef
bool addPort(PortBase &port)
long int usec() const
::CORBA::Boolean selectBaseLink(const char *linkname)
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
TimedOrientation3D m_sensorRpy
TimedDoubleSeq m_qRef
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
virtual ~ForwardKinematics()
Destructor.


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:49