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
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 }