[[SIGVerse with Python]]

The aim of this tutorial is to

--write a python module using pygame that captures mouse movements
--to present setting of environmental variables for writing Sigservices in Python
--import the module inside Sigservice and send data to Sigserver
--making a .sig  file and loading it into Sigviewer

To use this tutorial, the use needs to have boost.python libraries installed and configured on his visual studio project. I have shown how to do that in previous tutorial.

-Writing python module for capturing mouse movements using pygame

--Please install pygame from here. It should be set in your python path.
--Please install pygame from [[here:http://www.pygame.org/download.shtml]]. The version I am using is pygame-1.9.1.win32-py2.7.msi. It should be set in your python path.
--Please copy the following code and name it as "mouseModule.py"

#highlight(python){{

import sys
import time
import threading
import pygame
from pygame.locals import *

pos=[]

class App:
    def __init__(self):
        self._running = True
    
    def on_init(self):
        # pygame.init()
        # self._display_surf = pygame.display.set_mode(self.size, pygame.HWSURFACE | pygame.DOUBLEBUF)
        screen = pygame.display.set_mode((620, 400))
        self._running = True
        self.flag=0

    def on_event(self, event):
        if event.type == pygame.QUIT:
            self._running = False
    def on_loop(self):
        pass
    def on_render(self):
        pass
    def on_cleanup(self):
        pygame.quit()

    def on_mouseMotionNew(self):
        background_colour = (255,255,255)
        screen = pygame.display.set_mode((620, 400))
        screen.fill(background_colour)
        flag=0
        running=1
        num=[]
        # pos=[]
        posAbs=[]
        posRel=[]
        global pos

        while running:
            event = pygame.event.poll()
            if event.type == pygame.QUIT:
                running = 0
            if event.type == pygame.MOUSEBUTTONDOWN or event.type == pygame.MOUSEMOTION:
                (a,b,c)= pygame.mouse.get_pressed()
                if a == 1:
                    print "the left button is pressed, logging starts"
                    # print "mouse at (%d, %d)" % event.pos
                    flag=1
                    count=0         
           
                elif c==1:
                    print "the right button is pressed, logging stops"
                    flag=0
                    flagList=[flag]
                    yield flagList     

            if event.type == pygame.MOUSEMOTION and flag==1:
                # print "mouse at (%d, %d)" % event.pos
                # pos=list(event.pos)
                count=count+1       
                pos=list(pygame.mouse.get_rel())
                # print "relative pos is (%d, %d)" % pygame.mouse.get_rel()
                # num.append(pos)
                pygame.draw.circle(screen, (0,0,240),event.pos, 3,1)
                pygame.display.flip()

                if count==1:
                    pass
                else:

                    yield pos

        pygame.quit()
        # return num

if __name__ == "__main__" :
    theApp = App()
    a=theApp.on_mouseMotionNew()
    for i in range(0,100):  # Please modify this value to change the number of iterations.
        print next(a)
    pygame.quit()

}}

When you run this code, it captures 100 mouse movements and captures the relative distance between successive mouse movements. To change the iteration from 100 to some other number, please modify the counter. We shall now be importing this user defined module in our code.

-Create a Visual Studio project 

--I have used Visual Studio 2008. 
--Please use Ctl+Shft+N to create a New Project. Lets call it "My Service Test". 


-Setting environmental variables. 

You need to have SigService library on your disk. Please download it from here:
I am using  SIGService_v2-1-0 and its location on my disk is at F:\Sigverse\SIGService_v2-1-0\SIGService_v2-1-0

--1. Go to:  Configuration Properties-->C/C++-->General-->Additional Include Directories
Add: 

         "F:\Sigverse\SIGService_v2-1-0\SIGService_v2-1-0\include";
          C:\Python27\include;
         "C:\Boost\include\boost-1_54"

--2. Go to  Configuration Properties-->Linker-->General-->Additional Library Directories

"F:\Sigverse\SIGService_v2-1-0\SIGService_v2-1-0\lib\Debug";
C:\Python27\libs;
C:\Boost\lib\i386

--3. Go to  Configuration Properties-->Linker-->General-->Output File.

      $(OutDir)\$(ProjectName).sig

--4. Go to  Configuration Properties-->Linker-->Input-->Additional Dependencies

            SIGService.lib 
            C:\Python27\libs\python27.lib 
            C:\Boost\lib\i386\boost_python-vc90-mt-gd-1_54.lib


- Writing a source code C++ file, name it as "MyService.cpp". Please place the python module "mouseModule.py" in the same directory as C++ code. 

#highlight(c){{

#include "SIGService.h"
#include <boost/python.hpp>
#include <boost/detail/lightweight_test.hpp>
#include <iostream>
#include <string>


namespace py = 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 Mouse_Service : public sigverse::SIGService 
  { 
public: 
    Mouse_Service(std::string name); 
    ~Mouse_Service(); 
    double onAction(); 
    void onRecvMsg(sigverse::RecvMsgEvent &evt); 
//private: Joystick*  JS; //pointer to joystick object 
//private: HWND hWnd;  
 }; 
  Mouse_Service::Mouse_Service(std::string name) : SIGService(name){ 
            //JS = new Joystick(); 
            //hWnd = NULL; 
    }; 
Mouse_Service::~Mouse_Service() 
 { 
    this->disconnect(); 
 }
void Mouse_Service::onRecvMsg(sigverse::RecvMsgEvent &evt) 
{ 
}

double Mouse_Service::onAction() 
{ 

      Py_Initialize();

       try{

            py::object main_module = py::import( "__main__"); 
            py::object main_namespace = main_module.attr( "__dict__");
            py::exec( "print 'Hello, world'", main_namespace);
            py::exec( "print 'Hello, world'[3:5]" , main_namespace);
            py::exec( "print '.'.join(['1','2','3'])" , main_namespace);
            py::exec( "import mouseModule as im", main_namespace);
            py::exec( "obj=im.App()", main_namespace);
            py::exec( "a=obj.on_mouseMotionNew()" , main_namespace);

             int pos_n= 1000;
             float positionArray[2]={0,0};
            std::string msg( "");
             char tmp[32]={0};
            boost::python::list xl;
             // char c=getch();
            std::cout<< "press esc to exit! "<<std::endl;
             for(unsigned int i=0; i<pos_n; i++){
                   int flag=1;

                        py::object listOfPos = py::eval( "next(a)", main_namespace);
                         int lenList=len((listOfPos));
                         //std::cout << " length is " << lenList <<std::endl;

                         if (lenList==2){

                               for(unsigned int i=0; i<lenList; i++){
                               //std::cout << py::extract<double>((listOfPos)[i])<< std::endl;
                               //std::cout<<"The possition array before  is" << positionArray[0] << "  and   " << positionArray[1] << std::endl;
                               //std::cout<<py::extract<double>((listOfPos)[0]) <<std::endl;
                               //std::cout<<py::extract<double>((listOfPos)[1]) <<std::endl;

                              positionArray[i] = py::extract<double>((listOfPos)[i]);
                        
                              }
                        }

                   else {

                          positionArray[0]=0;
                          positionArray[1]=0;
                        
                        }

                        sprintf(tmp,  "%f,%f", positionArray[0], positionArray[1]);
                              std::cout<< "At i: " << i <<std::endl;
                              std::cout<< "The possition array is" << positionArray[0] << "  and   " << positionArray[1] << std::endl;
                              msg = std::string(tmp);
                              std::cout << "The sent msg is: " << msg << std::endl;
                               //srv.sendMsg("robot_test", msg);
                               this->sendMsg("robot_test" , msg);

                  }

      std::cout<< "exited: "<<std::endl;
      
      }

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

  return 0.00001;

}



int main(int argc, char** argv)
{
       // Create an instance of the service class with the specified service name
      Mouse_Service srv( "MouseService");
      std::string host = argv[1];
       unsigned short port = (unsigned short)(atoi(argv[2])); 
      srv.connect(host, port);
      srv.startLoop();
       // disconnect from the server
       //srv.disconnect();

       return 0;
}

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

The above code, imports the python module which captures relative positions of mouse movements. Those positions are then sent to Sigserver for moving objects inside Sigviewer.


-Debug the code and place the following files into project_name/Debug directory ( In my case it is F:\Sigverse\MyServiceTest\Debug)
--mouseModule.py
--boost_python-vc90-mt-gd-1_54.dll

-Please build the code, using F7. You can see that MyServiceTest.sig is created in the project_name/Debug directory.

-Now, lets go to the server side and see how our service works.

1. Create a project directory using 
          sigcreate.sh  NewProject

2.  Copy the following code and name it as LToolController.cpp

#highlight(c){{

#include "ControllerEvent.h"
#include "Controller.h"
#include "Logger.h"

class MyController : public Controller { 
public: 
  void onInit(InitEvent &evt); 
  double onAction(ActionEvent&);
  void onRecvMsg(RecvMsgEvent &evt);
  void onCollision(CollisionEvent &evt);  

private:
  bool accelerationValueRecieved;

}; 

static double xPos = 0;
static double yPos= 0;


static int messageCount;
static int onActionCount;
static int flag=1;
static Vector3d pos;

void MyController::onInit(InitEvent &evt) { 

  SimObj *obj = getObj(myname());
  // obj->addForce(0,0,-500);


} 
 
double MyController::onAction(ActionEvent &evt) { 
  //return 1.0;
  SimObj *obj = getObj(myname());

  double massOfTool; 
  massOfTool = obj->getMass();
  obj->getPosition(pos);
 
  return 0.00001;
     
}

std::string msg = " ";

void MyController::onRecvMsg(RecvMsgEvent &evt)
{

  SimObj *my = getObj(myname());

  char *all_msg = (char*)evt.getMsg();
  // std::string msg;
  msg= evt.getMsg();

  LOG_MSG((msg.c_str()));


  char xPosStr[10]=" ";
  char yPosStr[10]=" ";
  int result=0;

  result = sscanf(msg.c_str(), "%[^','],%[^',']", xPosStr, yPosStr );
  xPos = atof(xPosStr);
  yPos = atof(yPosStr);

  // LOG_MSG((" Position Received by Controller : %f %f ", xPos, yPos));
  //std::cout <<"xPos is" << xPos <<std::endl;

  //messageCount++;
  //std::cout << "The mesage count is" << messageCount <<std::endl;

  my->setPosition(pos.x(), pos.y(), pos.z() - yPos);

}


void MyController::onCollision(CollisionEvent &evt) {
}


extern "C" Controller * createController() { 
  return new MyController; 
} 

}}

--Run the code to make .so file.
        ./sigmake.sh    LToolController.cpp


3. Create the world file, which has a stick tool and object. Name it as "ToolsOnTable.xml"



#highlight(xml){{

<?xml version="1.0" encoding="utf8"?>
<world name="myworld1">
<gravity x="0.0" y="-9.8" z="0.0"/>



<instanciate class="robot_test.xml"> 

    <set-attr-value name="name" value="robot_test"/> 
    <set-attr-value name="language" value="c++"/> 
    <set-attr-value name="implementation" 
                    value="./LToolController.so"/> 
 
    <set-attr-value name="dynamics" value="true"/> 

    <set-attr-value name="mass" value="4.5"/> 
    <set-attr-value name="collision" value="true"/> 


    <set-attr-value name="x" value="10.0"/>
    <set-attr-value name="y" value="1.0"/>
    <set-attr-value name="z" value="40.0"/>

    <set-attr-value name="qw" value="0.707"/>
    <set-attr-value name="qx" value="0.707"/>
    <set-attr-value name="qy" value="0.0"/>
    <set-attr-value name="qz" value="0.0"/>


</instanciate>


<instanciate class="robot_box.xml">

   <set-attr-value name="name" value="box_001"/> 
  <set-attr-value name="mass" value="1.5"/>   
   <set-attr-value name="dynamics" value="true"/>
   <set-attr-value name="collision" value="true"/>
   <set-attr-value name="x" value="10.0"/>
   <set-attr-value name="y" value="1.0"/>
   <set-attr-value name="z" value="36.0"/>
   <set-attr-value name="qw" value="0.707"/>
   <set-attr-value name="qx" value="0.707"/>
   <set-attr-value name="qy" value="0.0"/>
   <set-attr-value name="qz" value="0.0"/>

</instanciate>

<worldParam erp="0.5" cfm="1e-8" autostep="true" quickstep="false" stepsize="0.01"/> 
<collisionParam mu="0.9" mu2="0.02" slip1="0.01" slip2="0.01" 
                  soft_erp="0.8" soft_cfm="0.00001" bounce="0.3" bounce_vel="5.0"/>

</world>


}} 

--The  robot_test.xml to create xml file for stick tool.

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

}}

--The robot_text.x3d to create stick tool. 

#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 rotation='0.0 0.0 1.0 1.57'>
               <Shape>
                 <Box size="2 16 2"/>
                 <Appearance>
                      <Material  diffuseColor='0.6 0.8 0.1' />
                 </Appearance>
               </Shape>
             </Transform>
           </ProtoInstance>
    
           <!-- <ProtoInstance name='Joint' DEF="JOINT2"  containerField='children'>
             <fieldValue name='name' value='JOINT2' />
             <fieldValue name='translation' value='-8.0 0.0 -2.0' />
             <fieldValue name='jointType' value='fixed' />
             <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 10.0 0.0' />
               <fieldValue name='mass' value='0.5' />
               <Transform rotation='0.0 0.0 0.0 0'>
                 <Shape>
                        <Box size="2 16 2"/>
                      <Appearance>
                        <Material  diffuseColor='0.6 0.6 0.1' />
                      </Appearance>
                 </Shape>
               </Transform>
             </ProtoInstance>
        </ProtoInstance>
  -->

       <!--    <ProtoInstance name='Joint' DEF="JOINT2"  containerField='children'>
          <fieldValue name='name' value='JOINT2' />
          <fieldValue name='translation' value='0.5 5.0 0.0' />
          <fieldValue name='jointType' value='fixed' />
          <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='8.0 2.0 0.0' />
            <fieldValue name='mass' value='0.5' />
            <Transform rotation='0.0 0.0 1.0 1.57'>
              <Shape>
          <Box size="2 16 2"/>
          <Appearance>
            <Material  diffuseColor='0.6 0.6 0.1' />
          </Appearance>
              </Shape>
            </Transform>
          </ProtoInstance>
        </ProtoInstance> -->


      </ProtoInstance>
      </ProtoInstance>
    </ProtoInstance>
  </Scene>
</X3D>

}}

--The robot_box.xml to create the xml file of small box.

#highlight(xml){{

<?xml version="1.0" encoding="utf8"?> 
<define-class name="robot_box" inherit="Agent.xml"> 
 
  <x3d> 
    <!--先ほど作成したモデルの読み込み--> 
    <filename>robot_box.x3d</filename> 
  </x3d> 
 
</define-class>

}}

--The robot_box.x3d to create a small box.

#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.1' />
             <Transform rotation='0.0 0.0 1.0 1.57'>
               <Shape>
                 <Box size="2 2 2"/>
                 <Appearance>
                      <Material  diffuseColor='0.0 1.0 0.0' />
                 </Appearance>
               </Shape>
             </Transform>
           </ProtoInstance>
    
           <!-- <ProtoInstance name='Joint' DEF="JOINT2"  containerField='children'>
             <fieldValue name='name' value='JOINT2' />
             <fieldValue name='translation' value='-8.0 0.0 -2.0' />
             <fieldValue name='jointType' value='fixed' />
             <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 10.0 0.0' />
               <fieldValue name='mass' value='0.5' />
               <Transform rotation='0.0 0.0 0.0 0'>
                 <Shape>
                        <Box size="2 16 2"/>
                      <Appearance>
                        <Material  diffuseColor='0.6 0.6 0.1' />
                      </Appearance>
                 </Shape>
               </Transform>
             </ProtoInstance>
        </ProtoInstance>
  -->

       <!--    <ProtoInstance name='Joint' DEF="JOINT2"  containerField='children'>
          <fieldValue name='name' value='JOINT2' />
          <fieldValue name='translation' value='0.5 5.0 0.0' />
          <fieldValue name='jointType' value='fixed' />
          <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='8.0 2.0 0.0' />
            <fieldValue name='mass' value='0.5' />
            <Transform rotation='0.0 0.0 1.0 1.57'>
              <Shape>
          <Box size="2 16 2"/>
          <Appearance>
            <Material  diffuseColor='0.6 0.6 0.1' />
          </Appearance>
              </Shape>
            </Transform>
          </ProtoInstance>
        </ProtoInstance> -->


      </ProtoInstance>
      </ProtoInstance>
    </ProtoInstance>
  </Scene>
</X3D>

}}

4. Please load the world file

     sigserver.sh -w ./ToolsOnTable.xml -p write_your_port_number


5. Go to Services-->Add-->MyServiceTest.sig and then Services-->Start

This shall add your mouse service.

6. Please click "Start" to run the simulation. 

7. When you use left click the mouse events are captured and their relative positions are recorded. Based on these relative positions, the tool moves which hits the target object.  On making right click, the event capturing is stopped.


#highlight(end)


Front page   New List of pages Search Recent changes   Help   RSS of recent changes