Computation of Inverse kinematics for 3dof planar robot
[
Front page
] [
New
|
List of pages
|
Search
|
Recent changes
|
Help
]
Start:
[[Tutorial]]
Up:[[Tutorial]] Previous:[[The first application...
#contents
* Creating the 3dof planar robot [#r207bdec]
** Creating the shape of robot using the OpenHRP format e...
Follow this tutorial [[Using OpenHRP Format:http://socio4...
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='cen...
<field accessType='inputOutput' type='MFNode' name='chil...
<field accessType='inputOutput' type='MFFloat' name='lli...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='SFString' name='na...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='SFVec3f' name='sca...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='MFFloat' name='sti...
<field accessType='inputOutput' type='SFVec3f' name='tra...
<field accessType='inputOutput' type='MFFloat' name='uli...
<field accessType='inputOutput' type='MFFloat' name='dh'...
<field accessType='inputOutput' type='SFString' name='jo...
<field accessType='inputOutput' type='SFFloat' name='joi...
<field accessType='inputOutput' type='SFVec3f' name='joi...
</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='sc...
<connect nodeField='translation' protoField='transla...
</IS>
</Transform>
</ProtoBody>
</ProtoDeclare>
<ProtoDeclare name='Segment' >
<ProtoInterface>
<field accessType='initializeOnly' type='SFVec3f' name='...
<field accessType='initializeOnly' type='SFVec3f' name='...
<field accessType='inputOutput' type='SFVec3f' name='cen...
<field accessType='inputOutput' type='MFNode' name='chil...
<field accessType='inputOutput' type='SFNode' name='coor...
<field accessType='inputOutput' type='MFNode' name='disp...
<field accessType='inputOutput' type='SFFloat' name='mas...
<field accessType='inputOutput' type='MFFloat' name='mom...
<field accessType='inputOutput' type='SFString' name='na...
<field accessType='inputOnly' type='MFNode' name='addChi...
<field accessType='inputOnly' type='MFNode' name='remove...
</ProtoInterface>
<ProtoBody>
<Group>
<IS>
<connect nodeField='children' protoField='children' />
<connect nodeField='addChildren' protoField='addChil...
<connect nodeField='removeChildren' protoField='remo...
<connect nodeField='bboxCenter' protoField='bboxCent...
<connect nodeField='bboxSize' protoField='bboxSize' />
</IS>
</Group>
</ProtoBody>
</ProtoDeclare>
<ProtoDeclare name='Humanoid' >
<ProtoInterface>
<field accessType='initializeOnly' type='SFVec3f' name='...
<field accessType='initializeOnly' type='SFVec3f' name='...
<field accessType='inputOutput' type='SFVec3f' name='cen...
<field accessType='inputOutput' type='MFNode' name='huma...
<field accessType='inputOutput' type='MFString' name='in...
<field accessType='inputOutput' type='MFNode' name='join...
<field accessType='inputOutput' type='SFString' name='na...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='SFVec3f' name='sca...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='MFNode' name='segm...
<field accessType='inputOutput' type='MFNode' name='site...
<field accessType='inputOutput' type='SFVec3f' name='tra...
<field accessType='inputOutput' type='SFString' name='ve...
<field accessType='inputOutput' type='MFNode' name='view...
</ProtoInterface>
<ProtoBody>
<Transform>
<IS>
<connect nodeField='bboxCenter' protoField='bboxCent...
<connect nodeField='bboxSize' protoField='bboxSize' />
<connect nodeField='center' protoField='center' />
<connect nodeField='rotation' protoField='rotation' />
<connect nodeField='scale' protoField='scale' />
<connect nodeField='scaleOrientation' protoField='sc...
<connect nodeField='translation' protoField='transla...
</IS>
<Group>
<IS>
<connect nodeField='children' protoField='viewpoin...
</IS>
</Group>
<Group>
<IS>
<connect nodeField='children' protoField='humanoid...
</IS>
</Group>
</Transform>
</ProtoBody>
</ProtoDeclare>
<ProtoInstance name='Humanoid' DEF="HRP1" contain...
<fieldValue name='info' value='"Test Joint"' />
<fieldValue name='name' value='sample' />
<fieldValue name='version' value='1.1' />
<ProtoInstance name='Joint' DEF="JOINT0" containerFi...
<fieldValue name='name' value='JOINT0' />
<fieldValue name='jointType' value='free' />
<fieldValue name='jointId' value='1.0' />
<ProtoInstance name='Joint' DEF="JOINT1" containerField...
<fieldValue name='name' value='JOINT1' />
<fieldValue name='jointId' value='2.0' />
<fieldValue name='jointType' value='fixed' />
<ProtoInstance name='Segment' DEF="LINK1" containe...
<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" containerFie...
<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" containe...
<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" containerF...
<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" containe...
<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="./Dynami...
<!--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="./ObjectC...
<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 T...
-Download the toolbox [[from:http://code.google.com/p/rob...
-Copy the transformation module [[from:http://www.lfd.uci...
-Place the transformations.py inside robot/ directory of ...
-Place the robot folder in your sigverse workspace direct...
-Use the following script and put in your sigverse worksp...
Note: Ideally you can put your python scripts and modules...
kinematicSigverseTest.py
#highlight(python){{
# import section
import sys
#you will need to change the path to your workspace direc...
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 robo...
robotRotation : rotation of the baseframe from t...
thetaList: list of theta corresponding to joint ...
The flow: I transform the quaternion to homogenous...
the rotation of the baseframe from the orgin.
'''
rot=T.quaternion_matrix(robotRotation) ## This is a...
endEffectorLocalPos=fkine(tlr,thetaList) # calculat...
pos=transl(endEffectorLocalPos) # transforms the ho...
pos=pos.flatten().tolist()[0] # converts matrix i...
pos=[pos[2],pos[1],-pos[0],1]
pos=array(pos)
globalEndEffectorPos= array(baseOrigin) + dot((rot)...
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 variabl...
using namespace std;
std::string parse_python_exception(); // functional decla...
template <typename T> string tostr(const T& t) { ostrings...
class MoveController : public Controller {
public:
static int s_nValue;
static double joint1Angle; // contains changes to the...
static double joint2Angle; // contains changes to the...
static double joint3Angle; // contains changes to the...
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 interp...
}
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 modul...
py::object main_namespace = main_module.attr("__dic...
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,%...
my->getPosition(baseFromOrigin);
LOG_MSG((" The initial robot position is (%f,...
main_module.attr("baseFromOrigin") = "[" + ...
py::exec("import ast", main_namespace);
py::exec("baseFromOrigin = ast.literal_eval(...
py::exec("baseFromOrigin.append(1)", main_na...
main_module.attr("baseFrameOrientation") = "...
py::exec("baseFrameOrientation = ast.literal...
}
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(MoveContr...
tostr(MoveController::...
py::exec("theta = ast.literal_eval(theta)", main_...
py::exec("print theta, type(theta) ", main_namesp...
py::exec("print baseFromOrigin, type(baseFromOrig...
//import the inverese kinematics module written i...
py::exec("import kinematicSigverseTest as K", mai...
// calling the forward kinematics function to cal...
py::object endEffPosList = py::eval("K.endEffecto...
int pos_n = len((endEffPosList));
for(unsigned int i=0; i<pos_n; i++){
std::cout << py::extract<double>((endEffPosLi...
positionArray[i] = py::extract<double>((endEf...
}
sprintf(tmp, "%f,%f,%f", positionArray[0], posit...
msg = std::string(tmp);
sendMsg("box_001", msg);
main_module.attr("targetPos")= "[15,26,11]";
py::exec("targetPos = ast.literal_eval(targetPos)...
}
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 <<...
}
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, *traceb...
// 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 in...
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...
// 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 stringifi...
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 erro...
}
// Parse lines from the traceback using the Python tr...
if(traceback_ptr != NULL){
py::handle<> h_tb(traceback_ptr);
// Load the traceback module and the format_tb fu...
py::object tb(py::import("traceback"));
py::object fmt_tb(tb.attr("format_tb"));
// Call format_tb to get a list of traceback stri...
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 ...
py::extract<std::string> returned(tb_str);
if(returned.check())
ret += ": " + returned();
else
ret += std::string(": Unparseable Python trac...
}
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() ...
char xChange[10], yChange[10], zChange[10];
int result;
result = sscanf(msg.c_str(), "%[^','],%[^','],%[^',']",...
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 b...
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 ...
# python version
INCDIRS += /usr/include/python2.7
INCDIRS += /usr/lib/python2.7/dist-packages/numpy/core/in...
INCDIR = $(foreach dir, $(INCDIRS), -I$(dir))
%.o: %.cpp
$(CPP) $(INCDIR) $(CFLAGS) $(CPPFLAGS) -c $< -o $@
#specifying of object file
OBJS = DynamicsController.so ObjectController.so ObjectCo...
HWOBJS= hello_world.o
all: $(OBJS) helloworld
DynamicsController.so: DynamicsController.cpp
g++ -DCONTROLLER -DNDEBUG -DUSE_ODE -DdDOUBLE -I$(SIG_S...
ObjectController.so: ObjectController.cpp
g++ -DCONTROLLER -DNDEBUG -DUSE_ODE -DdDOUBLE -I$(SIG_SR...
#$^ , $+ The names of all the prerequisites, with spaces ...
# 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 res...
#highlight(end)
Up:[[Tutorial]] Previous:[[The first application...
#counter
End:
[[Tutorial]]
Up:[[Tutorial]] Previous:[[The first application...
#contents
* Creating the 3dof planar robot [#r207bdec]
** Creating the shape of robot using the OpenHRP format e...
Follow this tutorial [[Using OpenHRP Format:http://socio4...
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='cen...
<field accessType='inputOutput' type='MFNode' name='chil...
<field accessType='inputOutput' type='MFFloat' name='lli...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='SFString' name='na...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='SFVec3f' name='sca...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='MFFloat' name='sti...
<field accessType='inputOutput' type='SFVec3f' name='tra...
<field accessType='inputOutput' type='MFFloat' name='uli...
<field accessType='inputOutput' type='MFFloat' name='dh'...
<field accessType='inputOutput' type='SFString' name='jo...
<field accessType='inputOutput' type='SFFloat' name='joi...
<field accessType='inputOutput' type='SFVec3f' name='joi...
</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='sc...
<connect nodeField='translation' protoField='transla...
</IS>
</Transform>
</ProtoBody>
</ProtoDeclare>
<ProtoDeclare name='Segment' >
<ProtoInterface>
<field accessType='initializeOnly' type='SFVec3f' name='...
<field accessType='initializeOnly' type='SFVec3f' name='...
<field accessType='inputOutput' type='SFVec3f' name='cen...
<field accessType='inputOutput' type='MFNode' name='chil...
<field accessType='inputOutput' type='SFNode' name='coor...
<field accessType='inputOutput' type='MFNode' name='disp...
<field accessType='inputOutput' type='SFFloat' name='mas...
<field accessType='inputOutput' type='MFFloat' name='mom...
<field accessType='inputOutput' type='SFString' name='na...
<field accessType='inputOnly' type='MFNode' name='addChi...
<field accessType='inputOnly' type='MFNode' name='remove...
</ProtoInterface>
<ProtoBody>
<Group>
<IS>
<connect nodeField='children' protoField='children' />
<connect nodeField='addChildren' protoField='addChil...
<connect nodeField='removeChildren' protoField='remo...
<connect nodeField='bboxCenter' protoField='bboxCent...
<connect nodeField='bboxSize' protoField='bboxSize' />
</IS>
</Group>
</ProtoBody>
</ProtoDeclare>
<ProtoDeclare name='Humanoid' >
<ProtoInterface>
<field accessType='initializeOnly' type='SFVec3f' name='...
<field accessType='initializeOnly' type='SFVec3f' name='...
<field accessType='inputOutput' type='SFVec3f' name='cen...
<field accessType='inputOutput' type='MFNode' name='huma...
<field accessType='inputOutput' type='MFString' name='in...
<field accessType='inputOutput' type='MFNode' name='join...
<field accessType='inputOutput' type='SFString' name='na...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='SFVec3f' name='sca...
<field accessType='inputOutput' type='SFRotation' name='...
<field accessType='inputOutput' type='MFNode' name='segm...
<field accessType='inputOutput' type='MFNode' name='site...
<field accessType='inputOutput' type='SFVec3f' name='tra...
<field accessType='inputOutput' type='SFString' name='ve...
<field accessType='inputOutput' type='MFNode' name='view...
</ProtoInterface>
<ProtoBody>
<Transform>
<IS>
<connect nodeField='bboxCenter' protoField='bboxCent...
<connect nodeField='bboxSize' protoField='bboxSize' />
<connect nodeField='center' protoField='center' />
<connect nodeField='rotation' protoField='rotation' />
<connect nodeField='scale' protoField='scale' />
<connect nodeField='scaleOrientation' protoField='sc...
<connect nodeField='translation' protoField='transla...
</IS>
<Group>
<IS>
<connect nodeField='children' protoField='viewpoin...
</IS>
</Group>
<Group>
<IS>
<connect nodeField='children' protoField='humanoid...
</IS>
</Group>
</Transform>
</ProtoBody>
</ProtoDeclare>
<ProtoInstance name='Humanoid' DEF="HRP1" contain...
<fieldValue name='info' value='"Test Joint"' />
<fieldValue name='name' value='sample' />
<fieldValue name='version' value='1.1' />
<ProtoInstance name='Joint' DEF="JOINT0" containerFi...
<fieldValue name='name' value='JOINT0' />
<fieldValue name='jointType' value='free' />
<fieldValue name='jointId' value='1.0' />
<ProtoInstance name='Joint' DEF="JOINT1" containerField...
<fieldValue name='name' value='JOINT1' />
<fieldValue name='jointId' value='2.0' />
<fieldValue name='jointType' value='fixed' />
<ProtoInstance name='Segment' DEF="LINK1" containe...
<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" containerFie...
<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" containe...
<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" containerF...
<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" containe...
<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="./Dynami...
<!--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="./ObjectC...
<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 T...
-Download the toolbox [[from:http://code.google.com/p/rob...
-Copy the transformation module [[from:http://www.lfd.uci...
-Place the transformations.py inside robot/ directory of ...
-Place the robot folder in your sigverse workspace direct...
-Use the following script and put in your sigverse worksp...
Note: Ideally you can put your python scripts and modules...
kinematicSigverseTest.py
#highlight(python){{
# import section
import sys
#you will need to change the path to your workspace direc...
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 robo...
robotRotation : rotation of the baseframe from t...
thetaList: list of theta corresponding to joint ...
The flow: I transform the quaternion to homogenous...
the rotation of the baseframe from the orgin.
'''
rot=T.quaternion_matrix(robotRotation) ## This is a...
endEffectorLocalPos=fkine(tlr,thetaList) # calculat...
pos=transl(endEffectorLocalPos) # transforms the ho...
pos=pos.flatten().tolist()[0] # converts matrix i...
pos=[pos[2],pos[1],-pos[0],1]
pos=array(pos)
globalEndEffectorPos= array(baseOrigin) + dot((rot)...
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 variabl...
using namespace std;
std::string parse_python_exception(); // functional decla...
template <typename T> string tostr(const T& t) { ostrings...
class MoveController : public Controller {
public:
static int s_nValue;
static double joint1Angle; // contains changes to the...
static double joint2Angle; // contains changes to the...
static double joint3Angle; // contains changes to the...
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 interp...
}
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 modul...
py::object main_namespace = main_module.attr("__dic...
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,%...
my->getPosition(baseFromOrigin);
LOG_MSG((" The initial robot position is (%f,...
main_module.attr("baseFromOrigin") = "[" + ...
py::exec("import ast", main_namespace);
py::exec("baseFromOrigin = ast.literal_eval(...
py::exec("baseFromOrigin.append(1)", main_na...
main_module.attr("baseFrameOrientation") = "...
py::exec("baseFrameOrientation = ast.literal...
}
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(MoveContr...
tostr(MoveController::...
py::exec("theta = ast.literal_eval(theta)", main_...
py::exec("print theta, type(theta) ", main_namesp...
py::exec("print baseFromOrigin, type(baseFromOrig...
//import the inverese kinematics module written i...
py::exec("import kinematicSigverseTest as K", mai...
// calling the forward kinematics function to cal...
py::object endEffPosList = py::eval("K.endEffecto...
int pos_n = len((endEffPosList));
for(unsigned int i=0; i<pos_n; i++){
std::cout << py::extract<double>((endEffPosLi...
positionArray[i] = py::extract<double>((endEf...
}
sprintf(tmp, "%f,%f,%f", positionArray[0], posit...
msg = std::string(tmp);
sendMsg("box_001", msg);
main_module.attr("targetPos")= "[15,26,11]";
py::exec("targetPos = ast.literal_eval(targetPos)...
}
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 <<...
}
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, *traceb...
// 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 in...
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...
// 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 stringifi...
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 erro...
}
// Parse lines from the traceback using the Python tr...
if(traceback_ptr != NULL){
py::handle<> h_tb(traceback_ptr);
// Load the traceback module and the format_tb fu...
py::object tb(py::import("traceback"));
py::object fmt_tb(tb.attr("format_tb"));
// Call format_tb to get a list of traceback stri...
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 ...
py::extract<std::string> returned(tb_str);
if(returned.check())
ret += ": " + returned();
else
ret += std::string(": Unparseable Python trac...
}
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() ...
char xChange[10], yChange[10], zChange[10];
int result;
result = sscanf(msg.c_str(), "%[^','],%[^','],%[^',']",...
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 b...
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 ...
# python version
INCDIRS += /usr/include/python2.7
INCDIRS += /usr/lib/python2.7/dist-packages/numpy/core/in...
INCDIR = $(foreach dir, $(INCDIRS), -I$(dir))
%.o: %.cpp
$(CPP) $(INCDIR) $(CFLAGS) $(CPPFLAGS) -c $< -o $@
#specifying of object file
OBJS = DynamicsController.so ObjectController.so ObjectCo...
HWOBJS= hello_world.o
all: $(OBJS) helloworld
DynamicsController.so: DynamicsController.cpp
g++ -DCONTROLLER -DNDEBUG -DUSE_ODE -DdDOUBLE -I$(SIG_S...
ObjectController.so: ObjectController.cpp
g++ -DCONTROLLER -DNDEBUG -DUSE_ODE -DdDOUBLE -I$(SIG_SR...
#$^ , $+ The names of all the prerequisites, with spaces ...
# 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 res...
#highlight(end)
Up:[[Tutorial]] Previous:[[The first application...
#counter
Page: