[[Tutorial]] Up:[[Tutorial]] Previous:[[The first application using Python]] Next:[[Import and use of user defined python modules]] #contents * Creating the 3dof planar robot [#r207bdec] ** Creating the shape of robot using the OpenHRP format explained here: [#n1dc039f] Follow this tutorial [[Using OpenHRP Format:http://socio4.iir.nii.ac.jp/wiki/jp/?%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%E3%83%BB%E3%83%80%E3%82%A4%E3%83%8A%E3%83%9F%E3%82%AF%E3%82%B9%EF%BC%88%E5%8D%98%E7%B4%94%E3%83%A2%E3%83%87%E3%83%AB%EF%BC%89]] robot_test.x3d #highlight(xml){{ <?xml version="1.0" encoding="UTF-8"?> <X3D profile="Immersive" version="3.0"> <Scene> <ProtoDeclare name='Joint' > <ProtoInterface> <field accessType='inputOutput' type='SFVec3f' name='center' value='0.0 0.0 0.0' /> <field accessType='inputOutput' type='MFNode' name='children'/> <field accessType='inputOutput' type='MFFloat' name='llimit' /> <field accessType='inputOutput' type='SFRotation' name='limitOrientation' value='0.0 0.0 1.0 0.0' /> <field accessType='inputOutput' type='SFString' name='name' value='' /> <field accessType='inputOutput' type='SFRotation' name='rotation' value='0.0 0.0 1.0 0.0' /> <field accessType='inputOutput' type='SFVec3f' name='scale' value='1.0 1.0 1.0' /> <field accessType='inputOutput' type='SFRotation' name='scaleOrientation' value='0.0 0.0 1.0 0.0' /> <field accessType='inputOutput' type='MFFloat' name='stiffness' value='0.0 ,0.0 ,0.0' /> <field accessType='inputOutput' type='SFVec3f' name='translation' value='0.0 0.0 0.0' /> <field accessType='inputOutput' type='MFFloat' name='ulimit' /> <field accessType='inputOutput' type='MFFloat' name='dh' value='0.0 ,0.0 ,0.0 ,0.0' /> <field accessType='inputOutput' type='SFString' name='jointType' value='' /> <field accessType='inputOutput' type='SFFloat' name='jointId' value='-1.0' /> <field accessType='inputOutput' type='SFVec3f' name='jointAxis' value='1.0 0.0 0.0' /> </ProtoInterface> <ProtoBody> <Transform> <IS> <connect nodeField='children' protoField='children' /> <connect nodeField='center' protoField='center' /> <connect nodeField='rotation' protoField='rotation' /> <connect nodeField='scale' protoField='scale' /> <connect nodeField='scaleOrientation' protoField='scaleOrientation' /> <connect nodeField='translation' protoField='translation' /> </IS> </Transform> </ProtoBody> </ProtoDeclare> <ProtoDeclare name='Segment' > <ProtoInterface> <field accessType='initializeOnly' type='SFVec3f' name='bboxCenter' value='0.0 0.0 0.0' /> <field accessType='initializeOnly' type='SFVec3f' name='bboxSize' value='-1.0 -1.0 -1.0' /> <field accessType='inputOutput' type='SFVec3f' name='centerOfMass' value='0.0 0.0 0.0' /> <field accessType='inputOutput' type='MFNode' name='children'/> <field accessType='inputOutput' type='SFNode' name='coord' /> <field accessType='inputOutput' type='MFNode' name='displacers' /> <field accessType='inputOutput' type='SFFloat' name='mass' value='0.0' /> <field accessType='inputOutput' type='MFFloat' name='momentsOfInertia' value='0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0' /> <field accessType='inputOutput' type='SFString' name='name' value='' /> <field accessType='inputOnly' type='MFNode' name='addChildren' /> <field accessType='inputOnly' type='MFNode' name='removeChildren'/> </ProtoInterface> <ProtoBody> <Group> <IS> <connect nodeField='children' protoField='children' /> <connect nodeField='addChildren' protoField='addChildren' /> <connect nodeField='removeChildren' protoField='removeChildren' /> <connect nodeField='bboxCenter' protoField='bboxCenter' /> <connect nodeField='bboxSize' protoField='bboxSize' /> </IS> </Group> </ProtoBody> </ProtoDeclare> <ProtoDeclare name='Humanoid' > <ProtoInterface> <field accessType='initializeOnly' type='SFVec3f' name='bboxCenter' value='0.0 0.0 0.0' /> <field accessType='initializeOnly' type='SFVec3f' name='bboxSize' value='-1.0 -1.0 -1.0' /> <field accessType='inputOutput' type='SFVec3f' name='center' value='0.0 0.0 0.0' /> <field accessType='inputOutput' type='MFNode' name='humanoidBody'/> <field accessType='inputOutput' type='MFString' name='info' /> <field accessType='inputOutput' type='MFNode' name='joints' /> <field accessType='inputOutput' type='SFString' name='name' value='' /> <field accessType='inputOutput' type='SFRotation' name='rotation' value='0.0 0.0 1.0 0.0' /> <field accessType='inputOutput' type='SFVec3f' name='scale' value='1.0 1.0 1.0' /> <field accessType='inputOutput' type='SFRotation' name='scaleOrientation' value='0.0 0.0 1.0 0.0' /> <field accessType='inputOutput' type='MFNode' name='segments'/> <field accessType='inputOutput' type='MFNode' name='sites' /> <field accessType='inputOutput' type='SFVec3f' name='translation' value='0.0 0.0 0.0' /> <field accessType='inputOutput' type='SFString' name='version' value='1.0' /> <field accessType='inputOutput' type='MFNode' name='viewpoints' /> </ProtoInterface> <ProtoBody> <Transform> <IS> <connect nodeField='bboxCenter' protoField='bboxCenter' /> <connect nodeField='bboxSize' protoField='bboxSize' /> <connect nodeField='center' protoField='center' /> <connect nodeField='rotation' protoField='rotation' /> <connect nodeField='scale' protoField='scale' /> <connect nodeField='scaleOrientation' protoField='scaleOrientation' /> <connect nodeField='translation' protoField='translation' /> </IS> <Group> <IS> <connect nodeField='children' protoField='viewpoints' /> </IS> </Group> <Group> <IS> <connect nodeField='children' protoField='humanoidBody' /> </IS> </Group> </Transform> </ProtoBody> </ProtoDeclare> <ProtoInstance name='Humanoid' DEF="HRP1" containerField='children'> <fieldValue name='info' value='"Test Joint"' /> <fieldValue name='name' value='sample' /> <fieldValue name='version' value='1.1' /> <ProtoInstance name='Joint' DEF="JOINT0" containerField='humanoidBody'> <fieldValue name='name' value='JOINT0' /> <fieldValue name='jointType' value='free' /> <fieldValue name='jointId' value='1.0' /> <ProtoInstance name='Joint' DEF="JOINT1" containerField='children'> <fieldValue name='name' value='JOINT1' /> <fieldValue name='jointId' value='2.0' /> <fieldValue name='jointType' value='fixed' /> <ProtoInstance name='Segment' DEF="LINK1" containerField='children'> <fieldValue name='name' value='LINK1' /> <fieldValue name='mass' value='0.5' /> <Transform> <Shape> <Box size="2 10 2"/> <Appearance> <Material diffuseColor='0.6 0.6 0.1' /> </Appearance> </Shape> </Transform> </ProtoInstance> <ProtoInstance name='Joint' DEF="JOINT2" containerField='children'> <fieldValue name='name' value='JOINT2' /> <fieldValue name='translation' value='0.0 5.0 0.0' /> <fieldValue name='jointType' value='rotate' /> <fieldValue name='jointAxis' value='1.0 0.0 0.0' /> <fieldValue name='jointId' value='3.0' /> <ProtoInstance name='Segment' DEF="LINK2" containerField='children'> <fieldValue name='name' value='LINK2' /> <fieldValue name='centerOfMass' value='0.0 5.0 0.0' /> <fieldValue name='mass' value='0.5' /> <Transform> <Shape> <Box size="2 10 2"/> <Appearance> <Material diffuseColor='0.6 0.6 0.1' /> </Appearance> </Shape> </Transform> </ProtoInstance> <ProtoInstance name='Joint' DEF="JOINT3" containerField='children'> <fieldValue name='name' value='JOINT3' /> <fieldValue name='translation' value='0.0 10.0 0.0' /> <fieldValue name='jointType' value='rotate' /> <fieldValue name='jointAxis' value='1.0 0.0 0.0' /> <fieldValue name='jointId' value='4.0' /> <ProtoInstance name='Segment' DEF="LINK3" containerField='children'> <fieldValue name='name' value='LINK3' /> <fieldValue name='centerOfMass' value='0.0 5.0 0.0' /> <fieldValue name='mass' value='0.5' /> <Transform> <Shape> <Box size="2 10 2"/> <Appearance> <Material diffuseColor='0.6 0.6 0.1' /> </Appearance> </Shape> </Transform> </ProtoInstance> </ProtoInstance> </ProtoInstance> </ProtoInstance> </ProtoInstance> </ProtoInstance> </Scene> </X3D> }} ** Create the XML file for the robot. [#xd3b374a] robot_test.xml #highlight(xml){{ <?xml version="1.0" encoding="utf8"?> <define-class name="robot_test" inherit="Agent.xml"> <x3d> <filename>robot_test.x3d</filename> </x3d> </define-class> }} * Create the world file [#ecef9913] DynamicsWorld.xml #highlight(xml){{ <?xml version="1.0" encoding="utf8"?> <world name="myworld11"> <gravity x="0.0" y="-980.7" z="0.0"/> <instanciate class="robot_test.xml"> <set-attr-value name="name" value="robot_test"/> <set-attr-value name="implementation" value="./DynamicsController.so"/> <!--dynamics true--> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="15.0"/> <set-attr-value name="y" value="5.0"/> <set-attr-value name="z" value="10.0"/> <set-attr-value name="language" value="c++"/> <set-attr-value name="collision" value="true"/> </instanciate> <instanciate class="seBox.xml"> <set-attr-value name="name" value="box_001"/> <set-attr-value name="mass" value="0.5"/> <set-attr-value name="language" value="c++"/> <set-attr-value name="implementation" value="./ObjectController.so"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="collision" value="true"/> <set-attr-value name="x" value="-20.0"/> <set-attr-value name="y" value="57.0"/> <set-attr-value name="z" value="15.0"/> </instanciate> </world> }} * Creating Inverse kinematics module using Python Robot Toolbox. [#pdc8f22f] -Download the toolbox [[from:http://code.google.com/p/robotics-toolbox-python/]] -Copy the transformation module [[from:http://www.lfd.uci.edu/~gohlke/code/transformations.py.html]]. Save it as transformations.py -Place the transformations.py inside robot/ directory of toolbox. -Place the robot folder in your sigverse workspace directory. -Use the following script and put in your sigverse workspace directory . Note: Ideally you can put your python scripts and modules anywhere on the disk, as far as you set your PYTHONPATH variable correctly. In this tutorial, we place them in our workspace. kinematicSigverseTest.py #highlight(python){{ # import section import sys #you will need to change the path to your workspace directory. sys.path.insert(0, 'path/to/sigverse/workspace/directory/') from numpy import * from robot import * # import robot models section from robot.testTwoLink import * # import the kinematics module from robot.kinematics import * from robot.transform import * from robot.manipulability import * import robot.transformations as T def endEffectorPos(baseOrigin,robotRotation,thetaList): ''' The arguments are: baseFromOrigin: distance of the base of the robot from the world origin. robotRotation : rotation of the baseframe from the orgin thetaList: list of theta corresponding to joint angles The flow: I transform the quaternion to homogenous rotation matrix, using famous transformations module to calculate the rotation of the baseframe from the orgin. ''' rot=T.quaternion_matrix(robotRotation) ## This is a better method to transform from quaternion to homogenous transformation. endEffectorLocalPos=fkine(tlr,thetaList) # calculation of forward kinematics. end effector pos relative to base frame of manipulator pos=transl(endEffectorLocalPos) # transforms the homogenous matrix to the postion information alone pos=pos.flatten().tolist()[0] # converts matrix into list pos=[pos[2],pos[1],-pos[0],1] pos=array(pos) globalEndEffectorPos= array(baseOrigin) + dot((rot),(pos)) # calculate end effector pos relative to origion. globalEndEffectorPos=list(globalEndEffectorPos) globalEndEffectorPos.pop() return globalEndEffectorPos }} *Creating the Controller for robot. [#f4ca5597] This controller DynamicsController.cpp #highlight(c++){{ #include "ControllerEvent.h" #include "Controller.h" #include "Logger.h" #include <iostream> #include <boost/python.hpp> #include <Python.h> #include <dlfcn.h> namespace py = boost::python; // create namespace variable for boost::python using namespace std; std::string parse_python_exception(); // functional declaration for exception handling template <typename T> string tostr(const T& t) { ostringstream os; os<<t; return os.str(); } // template to convert double variables to string class MoveController : public Controller { public: static int s_nValue; static double joint1Angle; // contains changes to the angle corresponding to Joint1 static double joint2Angle; // contains changes to the angle corresponding to Joint2 static double joint3Angle; // contains changes to the angle corresponding to Joint3 void onInit(InitEvent &evt); double onAction(ActionEvent&); }; int MoveController::s_nValue = 1; double MoveController::joint1Angle = 0; double MoveController::joint2Angle = 0; double MoveController::joint3Angle = 0; void MoveController::onInit(InitEvent &evt) { SimObj *my = getObj(myname()); dlopen("libpython2.7.so", RTLD_LAZY | RTLD_GLOBAL); Py_Initialize(); //initialization of the python interpreter } double MoveController::onAction(ActionEvent &evt) { SimObj *my = getObj(myname()); // variable declaratiom char tmp[32]; char tmp1[32]; std::string msg; std::string msg1; Vector3d nextPos; Vector3d finalPos; double positionArray[3]={0,0,0}; double pi=22/7; try{ // load the main module py::object main_module = py::import("__main__"); // load the dictionary object out of the main module to create a blank canvas on which python variables and functions can be executed. py::object main_namespace = main_module.attr("__dict__"); if (MoveController::s_nValue == 1 ) { Vector3d baseFromOrigin; Rotation rot; my->getRotation(rot); double qw = rot.qw(); double qx = rot.qx(); double qy = rot.qy(); double qz = rot.qz(); LOG_MSG((" The rotation onInit() is (%f, %f,%f,%f)", qw, qx, qy, qz )); my->getPosition(baseFromOrigin); LOG_MSG((" The initial robot position is (%f, %f,%f)", baseFromOrigin.x(), baseFromOrigin.y(), baseFromOrigin.z())); main_module.attr("baseFromOrigin") = "[" + tostr(baseFromOrigin.x())+" , "+ tostr(baseFromOrigin.y())+ " , " + tostr(baseFromOrigin.z()) + "]"; py::exec("import ast", main_namespace); py::exec("baseFromOrigin = ast.literal_eval(baseFromOrigin)", main_namespace); py::exec("baseFromOrigin.append(1)", main_namespace); main_module.attr("baseFrameOrientation") = "[" + tostr(qw)+" , "+ tostr(qx)+ " , " + tostr(qy) + " , " + tostr(qz)+ "]"; py::exec("baseFrameOrientation = ast.literal_eval(baseFrameOrientation)", main_namespace); } my->setJointAngle("JOINT2", 0.8); my->setJointAngle("JOINT3", -1.2); MoveController::joint1Angle = pi/2; MoveController::joint2Angle = 0.8; MoveController::joint3Angle = -1.2; main_module.attr("theta") = "[" + tostr(MoveController::joint1Angle)+" , "+ tostr(MoveController::joint2Angle)+ " , " + tostr(MoveController::joint3Angle) + "]"; py::exec("theta = ast.literal_eval(theta)", main_namespace); py::exec("print theta, type(theta) ", main_namespace); py::exec("print baseFromOrigin, type(baseFromOrigin)", main_namespace); //import the inverese kinematics module written in Python py::exec("import kinematicSigverseTest as K", main_namespace); // calling the forward kinematics function to calculate end effector position py::object endEffPosList = py::eval("K.endEffectorPos(baseFromOrigin,baseFrameOrientation,theta)", main_namespace); int pos_n = len((endEffPosList)); for(unsigned int i=0; i<pos_n; i++){ std::cout << py::extract<double>((endEffPosList)[i])<< std::endl; positionArray[i] = py::extract<double>((endEffPosList)[i]); } sprintf(tmp, "%f,%f,%f", positionArray[0], positionArray[1], positionArray[2] ); msg = std::string(tmp); sendMsg("box_001", msg); main_module.attr("targetPos")= "[15,26,11]"; py::exec("targetPos = ast.literal_eval(targetPos)", main_namespace); } catch(boost::python::error_already_set const &){ // Parse and output the exception std::string perror_str = parse_python_exception(); std::cout << "Error in Python: " << perror_str << std::endl; } MoveController::s_nValue ++; return 0.1; } extern "C" Controller * createController() { return new MoveController; } std::string parse_python_exception(){ PyObject *type_ptr = NULL, *value_ptr = NULL, *traceback_ptr = NULL; // Fetch the exception info from the Python C API PyErr_Fetch(&type_ptr, &value_ptr, &traceback_ptr); // Fallback error std::string ret("Unfetchable Python error"); // If the fetch got a type pointer, parse the type into the exception string if(type_ptr != NULL){ py::handle<> h_type(type_ptr); py::str type_pstr(h_type); // Extract the string from the boost::python object py::extract<std::string> e_type_pstr(type_pstr); // If a valid string extraction is available, use it // otherwise use fallback if(e_type_pstr.check()) ret = e_type_pstr(); else ret = "Unknown exception type"; } // Do the same for the exception value (the stringification of the exception) if(value_ptr != NULL){ py::handle<> h_val(value_ptr); py::str a(h_val); py::extract<std::string> returned(a); if(returned.check()) ret += ": " + returned(); else ret += std::string(": Unparseable Python error: "); } // Parse lines from the traceback using the Python traceback module if(traceback_ptr != NULL){ py::handle<> h_tb(traceback_ptr); // Load the traceback module and the format_tb function py::object tb(py::import("traceback")); py::object fmt_tb(tb.attr("format_tb")); // Call format_tb to get a list of traceback strings py::object tb_list(fmt_tb(h_tb)); // Join the traceback strings into a single string py::object tb_str(py::str("\n").join(tb_list)); // Extract the string, check the extraction, and fallback in necessary py::extract<std::string> returned(tb_str); if(returned.check()) ret += ": " + returned(); else ret += std::string(": Unparseable Python traceback"); } return ret; } }} *Creating the controller for object [#m76d0e0f] placing the cube on end endeffor of the robot. #highlight(c++){{ #include "ControllerEvent.h" #include "Controller.h" #include "Logger.h" static double xCha = 0; static double yCha= 0; static double zCha=0; class MyController : public Controller { public: void onInit(InitEvent &evt); double onAction(ActionEvent&); void onRecvMsg(RecvMsgEvent &evt); void onCollision(CollisionEvent &evt); }; void MyController::onInit(InitEvent &evt) { } double MyController::onAction(ActionEvent &evt) { SimObj *obj = getObj(myname()); obj->setPosition(xCha, yCha, zCha); return 0.1; } void MyController::onRecvMsg(RecvMsgEvent &evt) { SimObj *my = getObj(myname()); std::string msg = evt.getMsg(); //LOG_MSG((" Change in Position of Object onRecvMesg() : %s", msg.c_str())); char xChange[10], yChange[10], zChange[10]; int result; result = sscanf(msg.c_str(), "%[^','],%[^','],%[^',']", xChange, yChange, zChange ); xCha = atof(xChange); yCha = atof(yChange); zCha = atof(zChange); } void MyController::onCollision(CollisionEvent &evt) { } //自身のインスタンスをSIGVerseに返します extern "C" Controller * createController() { return new MyController; } }} * Creating the makefile [#n84f5fea] #highlight(c){{ #sigverse header SIG_SRC = $(SIGVERSE_PATH)/include/sigverse #all: $(OBJS) CC = gcc CPP = g++ AS = as LD = g++ AR = ar RANLIB = ranlib OBJCOPY = objcopy # external libraries and headers # change the next line to point to the location of your boost installation EXTERN_DIR = $home/pool EXTERN_LIBDIR = $(EXTERN_DIR)/lib EXTERN_INCDIR = $(EXTERN_DIR)/include EXTERN_BINDIR = $(EXTERN_DIR)/bin BOOST_PYTHON_LIB = $(EXTERN_LIBDIR)/libboost_python.a INCDIRS = . INCDIRS += $(EXTERN_INCDIR) # you may also need to change this directory if you want to pin to a specific # python version INCDIRS += /usr/include/python2.7 INCDIRS += /usr/lib/python2.7/dist-packages/numpy/core/include INCDIR = $(foreach dir, $(INCDIRS), -I$(dir)) %.o: %.cpp $(CPP) $(INCDIR) $(CFLAGS) $(CPPFLAGS) -c $< -o $@ #specifying of object file OBJS = DynamicsController.so ObjectController.so ObjectController2.so HWOBJS= hello_world.o all: $(OBJS) helloworld DynamicsController.so: DynamicsController.cpp g++ -DCONTROLLER -DNDEBUG -DUSE_ODE -DdDOUBLE -I$(SIG_SRC) -I$(SIG_SRC)/comm/controller -I$(INCDIR) -fPIC -shared -o $@ $^ $(LDFLAGS) -L$(EXTERN_LIBDIR) -lboost_python -lpython2.7 ObjectController.so: ObjectController.cpp g++ -DCONTROLLER -DNDEBUG -DUSE_ODE -DdDOUBLE -I$(SIG_SRC) -I$(SIG_SRC)/comm/controller -fPIC -shared -o $@ $< #$^ , $+ The names of all the prerequisites, with spaces between them. The value of $^ omits duplicate prerequisites, # while $+ retains them and preserves their order clean: rm -rf *.so *.o $(HWOBJS) hello_world }} Then type on console to compile the code: $make Now, you need to set the library path using. #highlight(c){{ export LD_LIBRARY_PATH=$HOME/pool/lib }} Then, load the xml file using sigviewer: $sigserver.sh -w ./DynamicsController.xml After the world loads, push "Start Button" to see the results of python code. #highlight(end) Up:[[Tutorial]] Previous:[[The first application using Python]] Next:[[Import and use of user defined python modules]]