PyBody.cpp
Go to the documentation of this file.
00001 #include <boost/python.hpp>
00002 #include <boost/foreach.hpp>
00003 #include <boost/range/value_type.hpp>
00004 #include <hrpModel/Link.h>
00005 #include "PySimulator.h"
00006 #include "PyLink.h"
00007 #include "PyBody.h"
00008 
00009 const char* PyBody::pybody_spec[] =
00010 {
00011     "implementation_id", "PyBody",
00012     "type_name",         "PyBody",
00013     "description",       "PyBody component",
00014     "version",           "0.1",
00015     "vendor",            "AIST",
00016     "category",          "Generic",
00017     "activity_type",     "DataFlowComponent",
00018     "max_instance",      "10",
00019     "language",          "C++",
00020     "lang_type",         "compile",
00021     // Configuration variables
00022 
00023     ""
00024 };
00025 
00026 PyBody::PyBody(RTC::Manager* manager) : BodyRTC(manager), simulator(NULL)
00027 {
00028 }
00029 
00030 PyBody::~PyBody() 
00031 {
00032     //std::cout << "~PyBody()" << std::endl;
00033 }
00034 
00035 void PyBody::setPosition(PyObject *v)
00036 {
00037     rootLink()->setPosition(v);
00038 }
00039 
00040 void PyBody::setRotation(PyObject *v)
00041 {
00042     rootLink()->setRotation(v);
00043 }
00044 
00045 void PyBody::setPosture(PyObject *v)
00046 {
00047     if (PySequence_Size(v) != numJoints()) return;
00048     for (unsigned int i=0; i<numJoints(); i++) {
00049         hrp::Link *j = joint(i);
00050         if (j) j->q = boost::python::extract<double>(PySequence_GetItem(v, i));
00051     }
00052     notifyChanged(KINEMATICS);
00053 }
00054 
00055 PyObject *PyBody::getPosition()
00056 {
00057     return rootLink()->getPosition();
00058 }
00059 
00060 PyObject *PyBody::getRotation()
00061 {
00062     return rootLink()->getRotation();
00063 }
00064 
00065 PyObject *PyBody::getPosture()
00066 {
00067     boost::python::list retval;
00068     for (unsigned int i=0; i<numJoints(); i++){
00069         hrp::Link *j = joint(i);
00070         double q = j ? j->q : 0;
00071         retval.append(boost::python::object(q));
00072     }
00073     return boost::python::incref(retval.ptr());
00074 }
00075 
00076 void PyBody::calcForwardKinematics()
00077 {
00078     Body::calcForwardKinematics();
00079 }
00080 
00081 PyLink *PyBody::rootLink()
00082 {
00083     return (PyLink *)Body::rootLink();
00084 }
00085 
00086 PyLink *PyBody::link(std::string name)
00087 {
00088     return (PyLink *)Body::link(name);
00089 }
00090 
00091 PyObject *PyBody::links()
00092 {
00093     boost::python::list retval;
00094     for (unsigned int i=0; i<numLinks(); i++){
00095         PyLink *l = (PyLink *)Body::link(i);
00096         retval.append(boost::python::ptr(l));
00097     }
00098     return boost::python::incref(retval.ptr());
00099 }
00100 
00101 PyLink *PyBody::joint(int i)
00102 {
00103     return (PyLink *)Body::joint(i);
00104 }
00105 PyObject *PyBody::joints()
00106 {
00107     boost::python::list retval;
00108     for (unsigned int i=0; i<numJoints(); i++){
00109         PyLink *l = (PyLink *)Body::joint(i);
00110         retval.append(boost::python::ptr(l));
00111     }
00112     return boost::python::incref(retval.ptr());
00113 }
00114 
00115 std::string PyBody::getName()
00116 {
00117     return Body::name();
00118 }
00119 
00120 PyObject *PyBody::calcCM()
00121 {
00122     hrp::Vector3 cm = Body::calcCM();
00123     boost::python::list retval;
00124     for (int i=0; i<3; i++){
00125         retval.append(boost::python::object(cm[i]));
00126     }
00127     return boost::python::incref(retval.ptr());
00128 }
00129 
00130 void PyBody::notifyChanged(int change)
00131 {
00132     switch(change){
00133     case STRUCTURE:
00134         updateLinkTree();
00135         break;
00136     case KINEMATICS:
00137         calcForwardKinematics();
00138         simulator->notifyChanged();
00139         break;
00140     }
00141 }
00142 
00143 void PyBody::setListener(PySimulator *i_sim)
00144 {
00145     simulator = i_sim;
00146 }
00147 
00148 template <class _Delete>
00149 void DummyDelete(RTC::RTObject_impl* rtc)
00150 {
00151 }
00152 
00153 void PyBody::moduleInit(RTC::Manager* manager)
00154 {
00155     coil::Properties profile(pybody_spec);
00156     manager->registerFactory(profile,
00157                              RTC::Create<PyBody>,
00158                              DummyDelete<PyBody>
00159                              //RTC::Delete<PyBody>
00160         );
00161 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55