v_repExtPivotCalibration.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::PivotCalibrationVrepPlugin> pivotCalibrationPG;
47 
48 
50  4, // Example Value // Parameter name
51  sim_lua_arg_string,0, // "RobotMillTip" , // RobotBaseName,
52  sim_lua_arg_string,0, // "RobotMillTipTarget" , // RobotTipName,
53  sim_lua_arg_string,0, // "RobotMillTip" , // RobotBaseName,
54  sim_lua_arg_string,0, // "RobotMillTipTarget" , // RobotTipName,
55 };
56 
57 std::string LUA_SIM_EXT_PIVOT_CALIB_START_CALL_TIP("number result=simExtPivotCalibStart(string ToolTipToModifyName, string ToolTipToMeasureName, string ToolBaseToModifyName, string ToolBaseToMeasureName)");
58 
59 
61 {
62  if (!pivotCalibrationPG) {
63 
64  BOOST_LOG_TRIVIAL(info) << "v_repExtPivotCalibration Starting Pivot Calibration Plugin Data Collection\n";
65 
66  CLuaFunctionData data;
67 
68  if (data.readDataFromLua(p,inArgs_PIVOT_CALIB_START,inArgs_PIVOT_CALIB_START[0],"simExtPivotCalibStart"))
69  {
70  std::vector<CLuaFunctionDataItem>* inData=data.getInDataPtr();
71  std::string ToolTipModifyName((inData->at(0 ).stringData[0]));
72  std::string ToolTipMeasureName((inData->at(1 ).stringData[0]));
73  std::string ToolBaseModifyName((inData->at(0 ).stringData[0]));
74  std::string ToolBaseMeasureName((inData->at(2 ).stringData[0]));
75  pivotCalibrationPG=std::make_shared<grl::PivotCalibrationVrepPlugin>(
76  std::make_tuple(ToolTipModifyName , ToolTipMeasureName, ToolBaseModifyName, ToolBaseMeasureName)
77 
78  );
79  pivotCalibrationPG->construct();
80 
81  }
82  else
83  {
84  pivotCalibrationPG=std::make_shared<grl::PivotCalibrationVrepPlugin>();
85  pivotCalibrationPG->construct();
86  }
87  }
88 }
89 
91  1, // Example Value // Parameter name
92  sim_lua_arg_string,0, // "RobotMillTip" , // RobotBaseName,
93 };
94 
95 std::string LUA_SIM_EXT_PIVOT_CALIB_ALGORITHM_CALL_TIP("number result=simExtPivotCalibAlgorithm(string AlgorithmName) -- Algorithm options are TWO_STEP_PROCEDURE and COMBINATORICAL_APPROACH");
96 
97 
99 {
100  if (!pivotCalibrationPG) {
101 
102  BOOST_LOG_TRIVIAL(info) << "v_repExtPivotCalibration Setting Algorithm\n";
103 
104  CLuaFunctionData data;
105 
106  if (pivotCalibrationPG.get()!=nullptr && data.readDataFromLua(p,inArgs_PIVOT_CALIB_ALGORITHM,inArgs_PIVOT_CALIB_ALGORITHM[0],"simExtPivotCalibAlgorithm"))
107  {
108  std::vector<CLuaFunctionDataItem>* inData=data.getInDataPtr();
109  std::string AlgorithmName((inData->at(0 ).stringData[0]));
110  pivotCalibrationPG->setAlgorithm(AlgorithmName);
111 
112  }
113  else
114  {
115  }
116  }
117 }
118 
120 {
121  BOOST_LOG_TRIVIAL(info) << "v_repExtPivotCalibration Starting Pivot Calibration Plugin Data Collection\n";
122  pivotCalibrationPG=std::make_shared<grl::PivotCalibrationVrepPlugin>();
123  pivotCalibrationPG->construct();
124 }
125 
127 {
128 
129  BOOST_LOG_TRIVIAL(info) << "Ending v_repExtPivotCalibration plugin\n";
130  pivotCalibrationPG.reset();
131 }
132 
134 {
135  if (pivotCalibrationPG) {
136  pivotCalibrationPG->addFrame();
137  }
138 }
139 
141 {
142  if (pivotCalibrationPG) {
143  pivotCalibrationPG->estimatePivotOffset();
144  }
145 }
146 
147 
149 {
150  if (pivotCalibrationPG) {
151  pivotCalibrationPG->applyEstimate();
152  }
153 }
154 
155 
157 {
158  if (pivotCalibrationPG) {
159  pivotCalibrationPG->restoreSensorPosition();
160  }
161 }
162 
163 /// @todo implement and connect up this function
164 /// Returns the current transform estimate in a format that vrep understands
166 {
167  if (pivotCalibrationPG) {
168  }
169 }
170 
171 
172 
173 // This is the plugin start routine (called just once, just after the plugin was loaded):
174 VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
175 {
176  // Dynamically load and bind V-REP functions:
177  // ******************************************
178  // 1. Figure out this plugin's directory:
179  char curDirAndFile[1024];
180 #ifdef _WIN32
181  GetModuleFileName(NULL,curDirAndFile,1023);
182  PathRemoveFileSpec(curDirAndFile);
183 #elif defined (__linux) || defined (__APPLE__)
184  getcwd(curDirAndFile, sizeof(curDirAndFile));
185 #endif
186  std::string currentDirAndPath(curDirAndFile);
187  // 2. Append the V-REP library's name:
188  std::string temp(currentDirAndPath);
189 #ifdef _WIN32
190  temp+="\\v_rep.dll";
191 #elif defined (__linux)
192  temp+="/libv_rep.so";
193 #elif defined (__APPLE__)
194  temp+="/libv_rep.dylib";
195 #endif /* __linux || __APPLE__ */
196  // 3. Load the V-REP library:
197  vrepLib=loadVrepLibrary(temp.c_str());
198  if (vrepLib==NULL)
199  {
200  BOOST_LOG_TRIVIAL(error) << "Error, could not find or correctly load the V-REP library. Cannot start 'PluginSkeleton' plugin.\n";
201  return(0); // Means error, V-REP will unload this plugin
202  }
204  {
205  BOOST_LOG_TRIVIAL(error) << "Error, could not find all required functions in the V-REP library. Cannot start 'PluginSkeleton' plugin.\n";
207  return(0); // Means error, V-REP will unload this plugin
208  }
209  // ******************************************
210 
211  // Check the version of V-REP:
212  // ******************************************
213  int vrepVer;
215  if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
216  {
217  BOOST_LOG_TRIVIAL(error) << "Sorry, your V-REP copy is somewhat old. Cannot start 'PluginSkeleton' plugin.\n";
219  return(0); // Means error, V-REP will unload this plugin
220  }
221  // ******************************************
222 
223 
224  int noArgs[]={0}; // no input arguments
227  simRegisterCustomLuaFunction("simExtPivotCalibStop","number result=simExtPivotCalibStop()",noArgs,LUA_SIM_EXT_PIVOT_CALIB_STOP);
228  simRegisterCustomLuaFunction("simExtPivotCalibReset","number result=simExtPivotCalibReset()",noArgs,LUA_SIM_EXT_PIVOT_CALIB_RESET);
229  simRegisterCustomLuaFunction("simExtPivotCalibAddFrame","number result=simExtPivotCalibAddFrame()",noArgs,LUA_SIM_EXT_PIVOT_CALIB_ADD_FRAME);
230  simRegisterCustomLuaFunction("simExtPivotCalibFindTransform","number result=simExtPivotCalibFindTransform()",noArgs,LUA_SIM_EXT_PIVOT_CALIB_FIND_TRANSFORM);
231  simRegisterCustomLuaFunction("simExtPivotCalibApplyTransform","number result=simExtPivotCalibApplyTransform()",noArgs,LUA_SIM_EXT_PIVOT_CALIB_APPLY_TRANSFORM);
232  simRegisterCustomLuaFunction("simExtPivotCalibRestoreSensorPosition","number result=simExtPivotCalibRestoreSensorPosition()",noArgs,LUA_SIM_EXT_PIVOT_CALIB_RESTORE_SENSOR_POSITION);
233 
234 
235  // ******************************************
236 
237  BOOST_LOG_TRIVIAL(info) << "Pivot Calibration plugin initialized. Build date/time: " << __DATE__ << " " << __TIME__ <<"\n";
238 
239  return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
240 }
241 
242 // This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
243 VREP_DLLEXPORT void v_repEnd()
244 {
245  // Here you could handle various clean-up tasks
246 
247  /////////////////////////
248  // PUT OBJECT RESET CODE HERE
249  // close out as necessary
250  ////////////////////
251 
252  pivotCalibrationPG.reset();
253 
254  unloadVrepLibrary(vrepLib); // release the library
255 }
256 
257 // This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
258 VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
259 { // This is called quite often. Just watch out for messages/events you want to handle
260  // Keep following 5 lines at the beginning and unchanged:
261  static bool refreshDlgFlag=true;
262  int errorModeSaved;
265  void* retVal=NULL;
266 
267  // Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here.
268  // For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
269  // in the V-REP user manual.
270 
271 
273  refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
274 
276  { // A custom menu bar entry was selected..
277  // here you could make a plugin's main dialog visible/invisible
278  }
279 
281  { // This message is sent each time the scene was rendered (well, shortly after) (very often)
282  // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
283 
284  int flags=auxiliaryData[0];
285  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
286  bool instanceSwitched=((flags&64)!=0);
287 
288  if (instanceSwitched)
289  {
290  // React to an instance switch here!!
291  }
292 
293  if (sceneContentChanged)
294  { // we actualize plugin objects for changes in the scene
295  refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here
296  }
297 
298 
299 
300  //...
301  //////////////
302  // PUT MAIN CODE HERE
303 
304  /////////////
305  if (simGetSimulationState() != sim_simulation_advancing_abouttostop) //checks if the simulation is still running
306  {
307  //if(pivotCalibrationPG) BOOST_LOG_TRIVIAL(info) << "current simulation time:" << simGetSimulationTime() << std::endl; // gets simulation time point
308  }
309  // make sure it is "right" (what does that mean?)
310 
311 
312  // find the v-rep C functions to do the following:
313  ////////////////////////////////////////////////////
314  // Use handles that were found at the "start" of this simulation running
315 
316  // next few Lines get the joint angles, torque, etc from the simulation
317  if (pivotCalibrationPG)// && PivotCalibrationPG->allHandlesSet == true // allHandlesSet now handled internally
318  {
319 
320  // run one loop synchronizing the arm and plugin
321  // pivotCalibrationPG->run_one();
322 
323  }
324  }
325 
327  { // The main script is about to be run (only called while a simulation is running (and not paused!))
328 
329  }
330 
332  { // Simulation is about to start
333 
334  /////////////////////////
335  // PUT OBJECT STARTUP CODE HERE
336  ////////////////////
337  // get the handles to all the objects, joints, etc that we need
338  /////////////////////
339  // simGetObjectHandle
340 
341  try {
342  //pivotCalibrationPG = std::make_shared<grl::PivotCalibrationVrepPlugin>();
343  //pivotCalibrationPG->construct();
344  //PivotCalibrationPG->run_one(); // for debugging purposes only
345  //PivotCalibrationPG.reset(); // for debugging purposes only
346  } catch (boost::exception& e){
347  // log the error and print it to the screen, don't release the exception
348  std::string initerr("v_repExtPivotCalibration plugin initialization error:\n" + boost::diagnostic_information(e));
349  simAddStatusbarMessage( initerr.c_str());
350  BOOST_LOG_TRIVIAL(error) << initerr;
351  }
352  }
353 
355  { // Simulation just ended
356 
357  /////////////////////////
358  // PUT OBJECT RESET CODE HERE
359  // close out as necessary
360  ////////////////////
361 
362  }
363 
365  { // A script called simOpenModule (by default the main script). Is only called during simulation.
366  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
367  {
368  // we arrive here only at the beginning of a simulation
369  }
370  }
371 
373  { // A script called simHandleModule (by default the main script). Is only called during simulation.
374  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
375  {
376  // we arrive here only while a simulation is running
377  }
378  }
379 
381  { // A script called simCloseModule (by default the main script). Is only called during simulation.
382  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
383  {
384  // we arrive here only at the end of a simulation
385  }
386  }
387 
389  { // Here the user switched the scene. React to this message in a similar way as you would react to a full
390  // scene content change. In this plugin example, we react to an instance switch by reacting to the
391  // sim_message_eventcallback_instancepass message and checking the bit 6 (64) of the auxiliaryData[0]
392  // (see here above)
393 
394  }
395 
397  { // Here we have a plugin that is broadcasting data (the broadcaster will also receive this data!)
398 
399  }
400 
402  { // 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)
403 
404  }
405 
406  // You can add many more messages to handle here
407 
408  if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
409  { // handle refresh of the plugin's dialogs
410  // ...
411  refreshDlgFlag=false;
412  }
413 
414  // Keep following unchanged:
415  simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
416  return(retVal);
417 }
418 
int getVrepProcAddresses(LIBRARY lib)
Definition: v_repLib.cpp:672
LIBRARY loadVrepLibrary(const char *pathAndFilename)
Definition: v_repLib.cpp:622
void LUA_SIM_EXT_PIVOT_CALIB_RESTORE_SENSOR_POSITION(SLuaCallBack *p)
bool readDataFromLua(const SLuaCallBack *p, const int *expectedArguments, int requiredArgumentCount, const char *functionName)
void LUA_SIM_EXT_PIVOT_CALIB_FIND_TRANSFORM(SLuaCallBack *p)
ptrSimSetIntegerParameter simSetIntegerParameter
Definition: v_repLib.cpp:51
void LUA_SIM_EXT_PIVOT_CALIB_RESET(SLuaCallBack *p)
void LUA_SIM_EXT_PIVOT_CALIB_STOP(SLuaCallBack *p)
const int inArgs_PIVOT_CALIB_START[]
void LUA_SIM_EXT_PIVOT_CALIB_GET_TRANSFORM(SLuaCallBack *p)
LIBRARY vrepLib
const int inArgs_PIVOT_CALIB_ALGORITHM[]
VREP_DLLEXPORT unsigned char v_repStart(void *reservedPointer, int reservedInt)
#define PLUGIN_VERSION
ptrSimGetIntegerParameter simGetIntegerParameter
Definition: v_repLib.cpp:52
void unloadVrepLibrary(LIBRARY lib)
Definition: v_repLib.cpp:641
VREP_DLLEXPORT void v_repEnd()
void LUA_SIM_EXT_PIVOT_CALIB_START(SLuaCallBack *p)
std::string LUA_SIM_EXT_PIVOT_CALIB_START_CALL_TIP("number result=simExtPivotCalibStart(string ToolTipToModifyName, string ToolTipToMeasureName, string ToolBaseToModifyName, string ToolBaseToMeasureName)")
ptrSimRegisterCustomLuaFunction simRegisterCustomLuaFunction
Definition: v_repLib.cpp:208
VREP_DLLEXPORT void * v_repMessage(int message, int *auxiliaryData, void *customData, int *replyData)
std::shared_ptr< grl::PivotCalibrationVrepPlugin > pivotCalibrationPG
std::string LUA_SIM_EXT_PIVOT_CALIB_ALGORITHM_CALL_TIP("number result=simExtPivotCalibAlgorithm(string AlgorithmName) -- Algorithm options are TWO_STEP_PROCEDURE and COMBINATORICAL_APPROACH")
void LUA_SIM_EXT_PIVOT_CALIB_APPLY_TRANSFORM(SLuaCallBack *p)
ptrSimGetSimulationState simGetSimulationState
Definition: v_repLib.cpp:106
std::vector< CLuaFunctionDataItem > * getInDataPtr()
void LUA_SIM_EXT_PIVOT_CALIB_ALGORITHM(SLuaCallBack *p)
void LUA_SIM_EXT_PIVOT_CALIB_ADD_FRAME(SLuaCallBack *p)
ptrSimAddStatusbarMessage simAddStatusbarMessage
Definition: v_repLib.cpp:117