PyLink.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <rtm/Manager.h>
3 #include <rtm/CorbaNaming.h>
4 #include <hrpModel/ModelLoaderUtil.h>
5 #include "hrpsys/util/GLutil.h"
6 #include "PyLink.h"
7 #include "PyBody.h"
8 #include "PyShape.h"
9 #include "PyUtil.h"
10 
12 {
13 }
14 
16 {
17  //std::cout << "~PyLink()" << std::endl;
18 }
19 
21 {
22  boost::python::list retval;
23  VectorToPyList(p, retval);
24  return boost::python::incref(retval.ptr());
25 }
26 
27 void PyLink::setPosition(PyObject *v)
28 {
29  if (PySequence_Size(v) != 3) return;
30  PyListToVector(v, p);
31  notifyChanged();
32 }
33 
35 {
36  boost::python::list retval;
37  const hrp::Matrix33 R = attitude();
38  Matrix33ToPyList(R, retval);
39  return boost::python::incref(retval.ptr());
40 }
41 
42 void PyLink::setRotation(PyObject *v)
43 {
45  int n = PySequence_Size(v);
46  if (n == 9){
47  PyListToMatrix33(v, R);
48  }else if (n == 4){
49  hrp::Vector3 axis;
50  for (int i=0; i<3; i++) {
51  axis(i) = boost::python::extract<double>(PySequence_GetItem(v, i));
52  }
53  double angle = boost::python::extract<double>(PySequence_GetItem(v, 3));
54  hrp::calcRodrigues(R, axis, angle);
55  }else if (n == 3){
56  hrp::Vector3 rpy;
57  PyListToVector(v, rpy);
58  hrp::calcRotFromRpy(R, rpy[0], rpy[1], rpy[2]);
59  }else{
60  return;
61  }
62  setAttitude(R);
63  notifyChanged();
64 }
65 
67 {
68  boost::python::list retval;
69  VectorToPyList(b, retval);
70  return boost::python::incref(retval.ptr());
71 }
72 
73 void PyLink::setRelPosition(PyObject *v)
74 {
75  if (PySequence_Size(v) != 3) return;
76  if (parent){
77  PyListToVector(v, b);
79  }else{
80  PyListToVector(v, p);
82  }
83  notifyChanged();
84 }
85 
87 {
88  boost::python::list retval;
89  Matrix33ToPyList(Rs, retval);
90  return boost::python::incref(retval.ptr());
91 }
92 
93 void PyLink::setRelRotation(PyObject *v)
94 {
95  int n = PySequence_Size(v);
96  if (n == 9){
97  PyListToMatrix33(v, Rs);
98  }else if (n == 4){
99  hrp::Vector3 axis;
100  for (int i=0; i<3; i++) {
101  axis(i) = boost::python::extract<double>(PySequence_GetItem(v, i));
102  }
103  double angle = boost::python::extract<double>(PySequence_GetItem(v, 3));
104  hrp::calcRodrigues(Rs, axis, angle);
105  }else if (n == 3){
106  hrp::Vector3 rpy;
107  PyListToVector(v, rpy);
108  hrp::calcRotFromRpy(Rs, rpy[0], rpy[1], rpy[2]);
109  }else{
110  return;
111  }
113  notifyChanged();
114 }
115 
117 {
118  return q;
119 }
120 
121 void PyLink::setPosture(double v)
122 {
123  q = v;
124  setQ(q);
125  notifyChanged();
126 }
127 
128 PyObject *PyLink::getCoM()
129 {
130  boost::python::list retval;
131  VectorToPyList(c, retval);
132  return boost::python::incref(retval.ptr());
133 }
134 
135 void PyLink::setCoM(PyObject *v)
136 {
137  if (PySequence_Size(v) != 3) return;
138  PyListToVector(v, c);
139 }
140 
142 {
143  boost::python::list retval;
144  VectorToPyList(a, retval);
145  notifyChanged();
146  return boost::python::incref(retval.ptr());
147 }
148 
149 void PyLink::setRotationAxis(PyObject *v)
150 {
151  if (PySequence_Size(v) != 3) return;
152  PyListToVector(v, a);
153 }
154 
156 {
157  boost::python::list retval;
158  VectorToPyList(d, retval);
159  return boost::python::incref(retval.ptr());
160 }
161 
163 {
164  if (PySequence_Size(v) != 3) return;
165  PyListToVector(v, d);
166 }
167 
169 {
170  boost::python::list retval;
171  Matrix33ToPyList(I, retval);
172  return boost::python::incref(retval.ptr());
173 }
174 
175 void PyLink::setInertia(PyObject *v)
176 {
177  if (PySequence_Size(v) != 9) return;
178  PyListToMatrix33(v, I);
179 }
180 
182 {
183  PyBody *pybody = dynamic_cast<PyBody *>(body);
185 }
186 
187 PyObject *PyLink::getLinVel()
188 {
189  boost::python::list retval;
190  VectorToPyList(v, retval);
191  return boost::python::incref(retval.ptr());
192 }
193 
194 void PyLink::setLinVel(PyObject *pyo)
195 {
196  if (PySequence_Size(pyo) != 3) return;
197  PyListToVector(pyo, v);
198 }
199 
200 PyObject *PyLink::getAngVel()
201 {
202  boost::python::list retval;
203  VectorToPyList(w, retval);
204  return boost::python::incref(retval.ptr());
205 }
206 
207 void PyLink::setAngVel(PyObject *v)
208 {
209  if (PySequence_Size(v) != 3) return;
210  PyListToVector(v, w);
211 }
212 
214 {
215  PyLink *l = new PyLink();
216  l->name = name;
217  addChild(l);
218  PyBody *pybody = dynamic_cast<PyBody *>(body);
220  return l;
221 }
222 
223 void PyLink::addShapeFromFile(std::string url)
224 {
226  std::string nameServer = manager->getConfig()["corba.nameservers"];
227  int comPos = nameServer.find(",");
228  if (comPos < 0){
229  comPos = nameServer.length();
230  }
231  nameServer = nameServer.substr(0, comPos);
232  RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
233 
234  OpenHRP::ModelLoader_var modelloader = hrp::getModelLoader(CosNaming::NamingContext::_duplicate(naming.getRootContext()));
235  OpenHRP::ModelLoader::ModelLoadOption opt;
236  opt.readImage = true;
237  opt.AABBdata.length(0);
238  opt.AABBtype = OpenHRP::ModelLoader::AABB_NUM;
239  OpenHRP::BodyInfo_var binfo = modelloader->getBodyInfoEx(url.c_str(), opt);
240  OpenHRP::LinkInfoSequence_var lis = binfo->links();
241  loadShapeFromLinkInfo(this, lis[0], binfo, createPyShape);
242 }
243 
244 PyShape *PyLink::addCube(double x, double y, double z)
245 {
246  PyShape *shape = new PyShape();
247  loadCube(shape, x,y,z);
248  addShape(shape);
249  return shape;
250 }
251 
253 {
254  return (PyLink *)parent;
255 }
256 
258 {
259  boost::python::list retval;
260  hrp::Link *l = child;
261  while(l){
262  retval.append(boost::python::object((PyLink *)l));
263  l = l->sibling;
264  }
265  return boost::python::incref(retval.ptr());
266 }
267 
268 void PyLink::setJointType(std::string type)
269 {
270  if(type == "fixed" ){
271  jointType = Link::FIXED_JOINT;
272  } else if(type == "free" ){
273  jointType = Link::FREE_JOINT;
274  } else if(type == "rotate" ){
275  jointType = Link::ROTATIONAL_JOINT;
276  } else if(type == "slide" ){
277  jointType = Link::SLIDE_JOINT;
278  } else {
279  jointType = Link::FREE_JOINT;
280  }
281 }
282 
283 std::string PyLink::getJointType()
284 {
285  switch(jointType){
286  case Link::FIXED_JOINT:
287  return "fixed";
288  case Link::FREE_JOINT:
289  return "free";
290  case Link::ROTATIONAL_JOINT:
291  return "rotate";
292  case Link::SLIDE_JOINT:
293  return "slide";
294  default:
295  return "";
296  }
297 }
298 
299 PyObject *PyLink::shapes()
300 {
301  boost::python::list retval;
302  for (size_t i=0; i<m_shapes.size(); i++){
303  retval.append(boost::python::ptr((PyShape *)m_shapes[i]));
304  }
305  return boost::python::incref(retval.ptr());
306 }
307 
309 {
310  return jointId;
311 }
312 
313 void PyLink::setJointId(int id)
314 {
315  jointId = id;
316  PyBody *pybody = dynamic_cast<PyBody *>(body);
318 }
static void Matrix33ToPyList(const hrp::Matrix33 &M, boost::python::list &l)
Definition: PyUtil.h:12
HRPMODEL_API OpenHRP::ModelLoader_var getModelLoader(CosNaming::NamingContext_var cxt)
HRP_UTIL_EXPORT void calcRotFromRpy(Matrix33 &out_R, double r, double p, double y)
manager
GLshape * createPyShape()
Definition: PyShape.cpp:67
CORBA::ORB_ptr getORB()
png_uint_32 i
void setRotation(double r, double p, double y)
static Manager & instance()
void loadCube(GLshape *shape, double x, double y, double z)
Definition: GLutil.cpp:141
Eigen::Vector3d Vector3
Eigen::Matrix3d Matrix33
coil::Properties & getConfig()
static void PyListToMatrix33(PyObject *pyo, hrp::Matrix33 &M)
Definition: PyUtil.h:29
void setPosition(double x, double y, double z)
HRP_UTIL_EXPORT void calcRodrigues(Matrix44 &out_R, const Vector3 &axis, double q)
Definition: PyBody.h:8
n
Definition: PyShape.h:7
naming
void VectorToPyList(const T &v, boost::python::list &l)
Definition: PyUtil.h:5
static int id
void loadShapeFromLinkInfo(GLlink *link, const OpenHRP::LinkInfo &i_li, OpenHRP::ShapeSetInfo_ptr i_ssinfo, GLshape *(*shapeFactory)())
Definition: GLutil.cpp:202
void notifyChanged(int change)
Definition: PyBody.cpp:130
void PyListToVector(PyObject *pyo, T &v)
Definition: PyUtil.h:22


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