PyLink.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <rtm/Manager.h>
00003 #include <rtm/CorbaNaming.h>
00004 #include <hrpModel/ModelLoaderUtil.h>
00005 #include "hrpsys/util/GLutil.h"
00006 #include "PyLink.h"
00007 #include "PyBody.h"
00008 #include "PyShape.h"
00009 #include "PyUtil.h"
00010 
00011 PyLink::PyLink()
00012 {
00013 }
00014 
00015 PyLink::~PyLink()
00016 {
00017     //std::cout << "~PyLink()" << std::endl;
00018 }
00019 
00020 PyObject *PyLink::getPosition()
00021 {
00022     boost::python::list retval;
00023     VectorToPyList(p, retval);
00024     return boost::python::incref(retval.ptr());
00025 }
00026 
00027 void PyLink::setPosition(PyObject *v)
00028 {
00029     if (PySequence_Size(v) != 3) return;
00030     PyListToVector(v, p);
00031     notifyChanged();
00032 }
00033 
00034 PyObject *PyLink::getRotation()
00035 {
00036     boost::python::list retval;
00037     const hrp::Matrix33 R = attitude();
00038     Matrix33ToPyList(R, retval);
00039     return boost::python::incref(retval.ptr());
00040 }
00041 
00042 void PyLink::setRotation(PyObject *v)
00043 {
00044     hrp::Matrix33 R;
00045     int n = PySequence_Size(v);
00046     if (n == 9){
00047         PyListToMatrix33(v, R);
00048     }else if (n == 4){
00049         hrp::Vector3 axis;
00050         for (int i=0; i<3; i++) {
00051             axis(i) = boost::python::extract<double>(PySequence_GetItem(v, i));
00052         }
00053         double angle = boost::python::extract<double>(PySequence_GetItem(v, 3));
00054         hrp::calcRodrigues(R, axis, angle);
00055     }else if (n == 3){
00056         hrp::Vector3 rpy;
00057         PyListToVector(v, rpy);
00058         hrp::calcRotFromRpy(R, rpy[0], rpy[1], rpy[2]);
00059     }else{
00060         return;
00061     }
00062     setAttitude(R);
00063     notifyChanged();
00064 }
00065 
00066 PyObject *PyLink::getRelPosition()
00067 {
00068     boost::python::list retval;
00069     VectorToPyList(b, retval);
00070     return boost::python::incref(retval.ptr());
00071 }
00072 
00073 void PyLink::setRelPosition(PyObject *v)
00074 {
00075     if (PySequence_Size(v) != 3) return;
00076     if (parent){
00077         PyListToVector(v, b);
00078         GLcoordinates::setPosition(b);
00079     }else{
00080         PyListToVector(v, p);
00081         GLcoordinates::setPosition(p);
00082     }
00083     notifyChanged();
00084 }
00085 
00086 PyObject *PyLink::getRelRotation()
00087 {
00088     boost::python::list retval;
00089     Matrix33ToPyList(Rs, retval);
00090     return boost::python::incref(retval.ptr());
00091 }
00092 
00093 void PyLink::setRelRotation(PyObject *v)
00094 {
00095     int n = PySequence_Size(v);
00096     if (n == 9){
00097         PyListToMatrix33(v, Rs);
00098     }else if (n == 4){
00099         hrp::Vector3 axis;
00100         for (int i=0; i<3; i++) {
00101             axis(i) = boost::python::extract<double>(PySequence_GetItem(v, i));
00102         }
00103         double angle = boost::python::extract<double>(PySequence_GetItem(v, 3));
00104         hrp::calcRodrigues(Rs, axis, angle);
00105     }else if (n == 3){
00106         hrp::Vector3 rpy;
00107         PyListToVector(v, rpy);
00108         hrp::calcRotFromRpy(Rs, rpy[0], rpy[1], rpy[2]);
00109     }else{
00110         return;
00111     }
00112     GLcoordinates::setRotation(Rs);
00113     notifyChanged();
00114 }
00115 
00116 double PyLink::getPosture()
00117 {
00118     return q;
00119 }
00120 
00121 void PyLink::setPosture(double v)
00122 {
00123     q = v;
00124     setQ(q);
00125     notifyChanged();
00126 }
00127 
00128 PyObject *PyLink::getCoM()
00129 {
00130     boost::python::list retval;
00131     VectorToPyList(c, retval);
00132     return boost::python::incref(retval.ptr());
00133 }
00134 
00135 void PyLink::setCoM(PyObject *v)
00136 {
00137     if (PySequence_Size(v) != 3) return;
00138     PyListToVector(v, c);
00139 }
00140 
00141 PyObject *PyLink::getRotationAxis()
00142 {
00143     boost::python::list retval;
00144     VectorToPyList(a, retval);
00145     notifyChanged();
00146     return boost::python::incref(retval.ptr());
00147 }
00148 
00149 void PyLink::setRotationAxis(PyObject *v)
00150 {
00151     if (PySequence_Size(v) != 3) return;
00152     PyListToVector(v, a);
00153 }
00154 
00155 PyObject *PyLink::getTranslationAxis()
00156 {
00157     boost::python::list retval;
00158     VectorToPyList(d, retval);
00159     return boost::python::incref(retval.ptr());
00160 }
00161 
00162 void PyLink::setTranslationAxis(PyObject *v)
00163 {
00164     if (PySequence_Size(v) != 3) return;
00165     PyListToVector(v, d);
00166 }
00167 
00168 PyObject *PyLink::getInertia()
00169 {
00170     boost::python::list retval;
00171     Matrix33ToPyList(I, retval);
00172     return boost::python::incref(retval.ptr());
00173 }
00174 
00175 void PyLink::setInertia(PyObject *v)
00176 {
00177     if (PySequence_Size(v) != 9) return;
00178     PyListToMatrix33(v, I);
00179 }
00180 
00181 void PyLink::notifyChanged()
00182 {
00183     PyBody *pybody = dynamic_cast<PyBody *>(body);
00184     pybody->notifyChanged(PyBody::KINEMATICS);
00185 }
00186 
00187 PyObject *PyLink::getLinVel()
00188 {
00189     boost::python::list retval;
00190     VectorToPyList(v, retval);
00191     return boost::python::incref(retval.ptr());
00192 }
00193 
00194 void PyLink::setLinVel(PyObject *pyo)
00195 {
00196     if (PySequence_Size(pyo) != 3) return;
00197     PyListToVector(pyo, v);
00198 }
00199 
00200 PyObject *PyLink::getAngVel()
00201 {
00202     boost::python::list retval;
00203     VectorToPyList(w, retval);
00204     return boost::python::incref(retval.ptr());
00205 }
00206 
00207 void PyLink::setAngVel(PyObject *v)
00208 {
00209     if (PySequence_Size(v) != 3) return;
00210     PyListToVector(v, w);
00211 }
00212 
00213 PyLink* PyLink::addChildLink(std::string name)
00214 {
00215     PyLink *l = new PyLink();
00216     l->name = name;
00217     addChild(l);
00218     PyBody *pybody = dynamic_cast<PyBody *>(body);
00219     pybody->notifyChanged(PyBody::STRUCTURE);
00220     return l;
00221 }
00222 
00223 void PyLink::addShapeFromFile(std::string url)
00224 {
00225     RTC::Manager* manager = &RTC::Manager::instance();
00226     std::string nameServer = manager->getConfig()["corba.nameservers"];
00227     int comPos = nameServer.find(",");
00228     if (comPos < 0){
00229         comPos = nameServer.length();
00230     }
00231     nameServer = nameServer.substr(0, comPos);
00232     RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
00233     
00234     OpenHRP::ModelLoader_var modelloader = hrp::getModelLoader(CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00235     OpenHRP::ModelLoader::ModelLoadOption opt;
00236     opt.readImage = true;
00237     opt.AABBdata.length(0);
00238     opt.AABBtype = OpenHRP::ModelLoader::AABB_NUM;
00239     OpenHRP::BodyInfo_var binfo = modelloader->getBodyInfoEx(url.c_str(), opt);
00240     OpenHRP::LinkInfoSequence_var lis = binfo->links();
00241     loadShapeFromLinkInfo(this, lis[0], binfo, createPyShape);
00242 }
00243 
00244 PyShape *PyLink::addCube(double x, double y, double z)
00245 {
00246     PyShape *shape = new PyShape();
00247     loadCube(shape, x,y,z);
00248     addShape(shape);
00249     return shape;
00250 }
00251 
00252 PyLink *PyLink::getParent()
00253 {
00254     return (PyLink *)parent;
00255 }
00256 
00257 PyObject *PyLink::getChildren()
00258 {
00259     boost::python::list retval;
00260     hrp::Link *l = child;
00261     while(l){
00262         retval.append(boost::python::object((PyLink *)l));
00263         l = l->sibling;
00264     }
00265     return boost::python::incref(retval.ptr());
00266 }
00267 
00268 void PyLink::setJointType(std::string type)
00269 {
00270     if(type == "fixed" ){
00271         jointType = Link::FIXED_JOINT;
00272     } else if(type == "free" ){
00273         jointType = Link::FREE_JOINT;
00274     } else if(type == "rotate" ){
00275         jointType = Link::ROTATIONAL_JOINT;
00276     } else if(type == "slide" ){
00277         jointType = Link::SLIDE_JOINT;
00278     } else {
00279         jointType = Link::FREE_JOINT;
00280     }
00281 }
00282 
00283 std::string PyLink::getJointType()
00284 {
00285     switch(jointType){
00286     case Link::FIXED_JOINT:
00287         return "fixed";
00288     case Link::FREE_JOINT:
00289         return "free";
00290     case Link::ROTATIONAL_JOINT:
00291         return "rotate";
00292     case Link::SLIDE_JOINT:
00293         return "slide";
00294     default:
00295         return "";
00296     }
00297 }
00298 
00299 PyObject *PyLink::shapes()
00300 {
00301     boost::python::list retval;
00302     for (size_t i=0; i<m_shapes.size(); i++){
00303         retval.append(boost::python::ptr((PyShape *)m_shapes[i]));
00304     }
00305     return boost::python::incref(retval.ptr());
00306 }
00307 
00308 int PyLink::getJointId()
00309 {
00310     return jointId; 
00311 }
00312 
00313 void PyLink::setJointId(int id)
00314 {
00315     jointId = id;
00316     PyBody *pybody = dynamic_cast<PyBody *>(body);
00317     pybody->notifyChanged(PyBody::STRUCTURE);
00318 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:18