v_repExtHandEyeCalibration.cpp
Go to the documentation of this file.
1 // Copyright 2006-2014 Coppelia Robotics GmbH. All rights reserved.
2 // marc@coppeliarobotics.com
3 // www.coppeliarobotics.com
4 //
5 // -------------------------------------------------------------------
6 // THIS FILE IS DISTRIBUTED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
7 // WARRANTY. THE USER WILL USE IT AT HIS/HER OWN RISK. THE ORIGINAL
8 // AUTHORS AND COPPELIA ROBOTICS GMBH WILL NOT BE LIABLE FOR DATA LOSS,
9 // DAMAGES, LOSS OF PROFITS OR ANY OTHER KIND OF LOSS WHILE USING OR
10 // MISUSING THIS SOFTWARE.
11 //
12 // You are free to use/modify/distribute this file for whatever purpose!
13 // -------------------------------------------------------------------
14 //
15 // This file was automatically created for V-REP release V3.2.0 on Feb. 3rd 2015
16 
17 
18 #include "luaFunctionData.h"
21 
22 #include "v_repLib.h"
23 
24 #ifdef _WIN32
25  #include <shlwapi.h>
26  #pragma comment(lib, "Shlwapi.lib")
27 #endif /* _WIN32 */
28 
29 #if defined (__linux) || defined (__APPLE__)
30  #include <unistd.h>
31  #include <string.h>
32  #define _stricmp(x,y) strcasecmp(x,y)
33 #endif
34 
35 #define PLUGIN_VERSION 1
36 
37 LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind
38 
39 
40 #define CONCAT(x,y,z) x y z
41 #define strConCat(x,y,z) CONCAT(x,y,z)
42 #define LUA_GET_SENSOR_DATA_COMMAND "simExtSkeleton_getSensorData"
43 
44 
45 
46 std::shared_ptr<grl::HandEyeCalibrationVrepPlugin> handEyeCalibrationPG;
47 
48 /*int getPathPosVectorFromObjectPose(int objectHandle, float relativeDistance) //This might be helpful if we decide to implement all the code in the plugin (instead of the lua script
49 {
50  float positionOnPath[3];
51  float eulerAngles[3];
52 
53  simGetPositionOnPath(objectHandle, relativeDistance, positionOnPath);
54  simGetOrientationOnPath(objectHandle, eulerAngles);
55  int o = getQuaternionFromEulerAngle(positionOnPath, eulerAngles);
56  return (p,o);
57 }
58 */
59 
60 
62  4, // Example Value // Parameter name
63  sim_lua_arg_string,0, // "RobotMillTip" , // RobotBaseName,
64  sim_lua_arg_string,0, // "RobotMillTipTarget" , // RobotTipName,
65  sim_lua_arg_string,0, // "Robotiiwa" , // OpticalTrackerBaseName,
66  sim_lua_arg_string,0, // "tcp://0.0.0.0:30010" , // OpticalTrackerTipName
67 };
68 
69 std::string LUA_SIM_EXT_HAND_EYE_CALIB_START_CALL_TIP("number result=simExtHandEyeCalibStart(string RobotBaseName , string RobotTipName, string OpticalTrackerBaseName, string OpticalTrackerDetectedObjectName) -- KukaCommandMode options are JAVA and FRI");
70 
71 
73 {
74  if (!handEyeCalibrationPG) {
75 
76  BOOST_LOG_TRIVIAL(info) << "v_repExtHandEyeCalibration Starting Hand Eye Calibration Plugin Data Collection\n";
77 
78  CLuaFunctionData data;
79 
80  if (data.readDataFromLua(p,inArgs_HAND_EYE_CALIB_START,inArgs_HAND_EYE_CALIB_START[0],"simExtHandEyeCalibStart"))
81  {
82  std::vector<CLuaFunctionDataItem>* inData=data.getInDataPtr();
83  std::string RobotBaseName((inData->at(0 ).stringData[0]));
84  std::string RobotTipName((inData->at(1 ).stringData[0]));
85  std::string OpticalTrackerBaseName((inData->at(2 ).stringData[0]));
86  std::string OpticalTrackerDetectedObjectName(inData->at(3 ).stringData[0]);
87  handEyeCalibrationPG=std::make_shared<grl::HandEyeCalibrationVrepPlugin>(
88  std::make_tuple(RobotBaseName , RobotTipName, OpticalTrackerBaseName, OpticalTrackerDetectedObjectName)
89 
90  );
91  handEyeCalibrationPG->construct();
92 
93  }
94  else
95  {
96  handEyeCalibrationPG=std::make_shared<grl::HandEyeCalibrationVrepPlugin>();
97  handEyeCalibrationPG->construct();
98  }
99  }
100 }
101 
103 {
104  BOOST_LOG_TRIVIAL(info) << "v_repExtHandEyeCalibration Starting Hand Eye Calibration Plugin Data Collection\n";
105  handEyeCalibrationPG=std::make_shared<grl::HandEyeCalibrationVrepPlugin>();
106  handEyeCalibrationPG->construct();
107 }
108 
110 {
111 
112  BOOST_LOG_TRIVIAL(info) << "Ending v_repExtHandEyeCalibration plugin\n";
113  handEyeCalibrationPG.reset();
114 }
115 
117 {
118  if (handEyeCalibrationPG) {
119  handEyeCalibrationPG->addFrame();
120  }
121 }
122 
124 {
125  if (handEyeCalibrationPG) {
126  handEyeCalibrationPG->estimateHandEyeScrew();
127  }
128 }
129 
130 
132 {
133  if (handEyeCalibrationPG) {
134  handEyeCalibrationPG->applyEstimate();
135  }
136 }
137 
138 
140 {
141  if (handEyeCalibrationPG) {
142  handEyeCalibrationPG->restoreSensorPosition();
143  }
144 }
145 
146 /// @todo implement and connect up this function
147 /// Returns the current transform estimate in a format that vrep understands
149 {
150  if (handEyeCalibrationPG) {
151  }
152 }
153 
154 
155 
156 // This is the plugin start routine (called just once, just after the plugin was loaded):
157 VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
158 {
159  // Dynamically load and bind V-REP functions:
160  // ******************************************
161  // 1. Figure out this plugin's directory:
162  char curDirAndFile[1024];
163 #ifdef _WIN32
164  GetModuleFileName(NULL,curDirAndFile,1023);
165  PathRemoveFileSpec(curDirAndFile);
166 #elif defined (__linux) || defined (__APPLE__)
167  getcwd(curDirAndFile, sizeof(curDirAndFile));
168 #endif
169  std::string currentDirAndPath(curDirAndFile);
170  // 2. Append the V-REP library's name:
171  std::string temp(currentDirAndPath);
172 #ifdef _WIN32
173  temp+="\\v_rep.dll";
174 #elif defined (__linux)
175  temp+="/libv_rep.so";
176 #elif defined (__APPLE__)
177  temp+="/libv_rep.dylib";
178 #endif /* __linux || __APPLE__ */
179  // 3. Load the V-REP library:
180  vrepLib=loadVrepLibrary(temp.c_str());
181  if (vrepLib==NULL)
182  {
183  BOOST_LOG_TRIVIAL(error) << "Error, could not find or correctly load the V-REP library. Cannot start 'PluginSkeleton' plugin.\n";
184  return(0); // Means error, V-REP will unload this plugin
185  }
187  {
188  BOOST_LOG_TRIVIAL(error) << "Error, could not find all required functions in the V-REP library. Cannot start 'PluginSkeleton' plugin.\n";
190  return(0); // Means error, V-REP will unload this plugin
191  }
192  // ******************************************
193 
194  // Check the version of V-REP:
195  // ******************************************
196  int vrepVer;
198  if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
199  {
200  BOOST_LOG_TRIVIAL(error) << "Sorry, your V-REP copy is somewhat old. Cannot start 'PluginSkeleton' plugin.\n";
202  return(0); // Means error, V-REP will unload this plugin
203  }
204  // ******************************************
205 
206 
207  int noArgs[]={0}; // no input arguments
208  simRegisterCustomLuaFunction("simExtHandEyeCalibStart","number result=simExtHandEyeCalibStart()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_START);
209  simRegisterCustomLuaFunction("simExtHandEyeCalibStop","number result=simExtHandEyeCalibStop()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_STOP);
210  simRegisterCustomLuaFunction("simExtHandEyeCalibReset","number result=simExtHandEyeCalibReset()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_RESET);
211  simRegisterCustomLuaFunction("simExtHandEyeCalibAddFrame","number result=simExtHandEyeCalibAddFrame()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_ADD_FRAME);
212  simRegisterCustomLuaFunction("simExtHandEyeCalibFindTransform","number result=simExtHandEyeCalibFindTransform()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_FIND_TRANSFORM);
213  simRegisterCustomLuaFunction("simExtHandEyeCalibApplyTransform","number result=simExtHandEyeCalibApplyTransform()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM);
214  simRegisterCustomLuaFunction("simExtHandEyeCalibRestoreSensorPosition","number result=simExtHandEyeCalibRestoreSensorPosition()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_RESTORE_SENSOR_POSITION);
215 
216 
217  // ******************************************
218 
219  BOOST_LOG_TRIVIAL(info) << "Hand Eye Calibration plugin initialized. Build date/time: " << __DATE__ << " " << __TIME__ <<"\n";
220 
221  return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
222 }
223 
224 // This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
225 VREP_DLLEXPORT void v_repEnd()
226 {
227  // Here you could handle various clean-up tasks
228 
229  /////////////////////////
230  // PUT OBJECT RESET CODE HERE
231  // close out as necessary
232  ////////////////////
233 
234  handEyeCalibrationPG.reset();
235 
236  unloadVrepLibrary(vrepLib); // release the library
237 }
238 
239 // This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
240 VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
241 { // This is called quite often. Just watch out for messages/events you want to handle
242  // Keep following 5 lines at the beginning and unchanged:
243  static bool refreshDlgFlag=true;
244  int errorModeSaved;
247  void* retVal=NULL;
248 
249  // Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here.
250  // For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
251  // in the V-REP user manual.
252 
253 
255  refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
256 
258  { // A custom menu bar entry was selected..
259  // here you could make a plugin's main dialog visible/invisible
260  }
261 
263  { // This message is sent each time the scene was rendered (well, shortly after) (very often)
264  // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
265 
266  int flags=auxiliaryData[0];
267  bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
268  bool instanceSwitched=((flags&64)!=0);
269 
270  if (instanceSwitched)
271  {
272  // React to an instance switch here!!
273  }
274 
275  if (sceneContentChanged)
276  { // we actualize plugin objects for changes in the scene
277  refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here
278  }
279 
280 
281 
282  //...
283  //////////////
284  // PUT MAIN CODE HERE
285 
286  /////////////
287  if (simGetSimulationState() != sim_simulation_advancing_abouttostop) //checks if the simulation is still running
288  {
289  //if(handEyeCalibrationPG) BOOST_LOG_TRIVIAL(info) << "current simulation time:" << simGetSimulationTime() << std::endl; // gets simulation time point
290  }
291  // make sure it is "right" (what does that mean?)
292 
293 
294  // find the v-rep C functions to do the following:
295  ////////////////////////////////////////////////////
296  // Use handles that were found at the "start" of this simulation running
297 
298  // next few Lines get the joint angles, torque, etc from the simulation
299  if (handEyeCalibrationPG)// && HandEyeCalibrationPG->allHandlesSet == true // allHandlesSet now handled internally
300  {
301 
302  // run one loop synchronizing the arm and plugin
303  // handEyeCalibrationPG->run_one();
304 
305  }
306  }
307 
309  { // The main script is about to be run (only called while a simulation is running (and not paused!))
310 
311  }
312 
314  { // Simulation is about to start
315 
316  /////////////////////////
317  // PUT OBJECT STARTUP CODE HERE
318  ////////////////////
319  // get the handles to all the objects, joints, etc that we need
320  /////////////////////
321  // simGetObjectHandle
322 
323  try {
324  //handEyeCalibrationPG = std::make_shared<grl::HandEyeCalibrationVrepPlugin>();
325  //handEyeCalibrationPG->construct();
326  //HandEyeCalibrationPG->run_one(); // for debugging purposes only
327  //HandEyeCalibrationPG.reset(); // for debugging purposes only
328  } catch (boost::exception& e){
329  // log the error and print it to the screen, don't release the exception
330  std::string initerr("v_repExtHandEyeCalibration plugin initialization error:\n" + boost::diagnostic_information(e));
331  simAddStatusbarMessage( initerr.c_str());
332  BOOST_LOG_TRIVIAL(error) << initerr;
333  }
334  }
335 
337  { // Simulation just ended
338 
339  /////////////////////////
340  // PUT OBJECT RESET CODE HERE
341  // close out as necessary
342  ////////////////////
343 
344  }
345 
347  { // A script called simOpenModule (by default the main script). Is only called during simulation.
348  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
349  {
350  // we arrive here only at the beginning of a simulation
351  }
352  }
353 
355  { // A script called simHandleModule (by default the main script). Is only called during simulation.
356  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
357  {
358  // we arrive here only while a simulation is running
359  }
360  }
361 
363  { // A script called simCloseModule (by default the main script). Is only called during simulation.
364  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
365  {
366  // we arrive here only at the end of a simulation
367  }
368  }
369 
371  { // Here the user switched the scene. React to this message in a similar way as you would react to a full
372  // scene content change. In this plugin example, we react to an instance switch by reacting to the
373  // sim_message_eventcallback_instancepass message and checking the bit 6 (64) of the auxiliaryData[0]
374  // (see here above)
375 
376  }
377 
379  { // Here we have a plugin that is broadcasting data (the broadcaster will also receive this data!)
380 
381  }
382 
384  { // The scene is about to be saved. If required do some processing here (e.g. add custom scene data to be serialized with the scene)
385 
386  }
387 
388  // You can add many more messages to handle here
389 
390  if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
391  { // handle refresh of the plugin's dialogs
392  // ...
393  refreshDlgFlag=false;
394  }
395 
396  // Keep following unchanged:
397  simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
398  return(retVal);
399 }
400 
std::shared_ptr< grl::HandEyeCalibrationVrepPlugin > handEyeCalibrationPG
void LUA_SIM_EXT_HAND_EYE_CALIB_ADD_FRAME(SLuaCallBack *p)
int getVrepProcAddresses(LIBRARY lib)
Definition: v_repLib.cpp:672
LIBRARY loadVrepLibrary(const char *pathAndFilename)
Definition: v_repLib.cpp:622
VREP_DLLEXPORT void v_repEnd()
bool readDataFromLua(const SLuaCallBack *p, const int *expectedArguments, int requiredArgumentCount, const char *functionName)
void LUA_SIM_EXT_HAND_EYE_CALIB_STOP(SLuaCallBack *p)
void LUA_SIM_EXT_HAND_EYE_CALIB_GET_TRANSFORM(SLuaCallBack *p)
void LUA_SIM_EXT_HAND_EYE_CALIB_RESTORE_SENSOR_POSITION(SLuaCallBack *p)
ptrSimSetIntegerParameter simSetIntegerParameter
Definition: v_repLib.cpp:51
VREP_DLLEXPORT void * v_repMessage(int message, int *auxiliaryData, void *customData, int *replyData)
std::string LUA_SIM_EXT_HAND_EYE_CALIB_START_CALL_TIP("number result=simExtHandEyeCalibStart(string RobotBaseName , string RobotTipName, string OpticalTrackerBaseName, string OpticalTrackerDetectedObjectName) -- KukaCommandMode options are JAVA and FRI")
void LUA_SIM_EXT_HAND_EYE_CALIB_START(SLuaCallBack *p)
void LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM(SLuaCallBack *p)
ptrSimGetIntegerParameter simGetIntegerParameter
Definition: v_repLib.cpp:52
void unloadVrepLibrary(LIBRARY lib)
Definition: v_repLib.cpp:641
VREP_DLLEXPORT unsigned char v_repStart(void *reservedPointer, int reservedInt)
#define PLUGIN_VERSION
void LUA_SIM_EXT_HAND_EYE_CALIB_RESET(SLuaCallBack *p)
ptrSimRegisterCustomLuaFunction simRegisterCustomLuaFunction
Definition: v_repLib.cpp:208
const int inArgs_HAND_EYE_CALIB_START[]
LIBRARY vrepLib
void LUA_SIM_EXT_HAND_EYE_CALIB_FIND_TRANSFORM(SLuaCallBack *p)
ptrSimGetSimulationState simGetSimulationState
Definition: v_repLib.cpp:106
std::vector< CLuaFunctionDataItem > * getInDataPtr()
ptrSimAddStatusbarMessage simAddStatusbarMessage
Definition: v_repLib.cpp:117