v_repExtKukaLBRiiwa.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 #include "luaFunctionData.h"
18 #include "v_repExtKukaLBRiiwa.h"
20 
21 
22 #ifdef _WIN32
23  #include <shlwapi.h>
24  #pragma comment(lib, "Shlwapi.lib")
25 #endif /* _WIN32 */
26 
27 #if defined (__linux) || defined (__APPLE__)
28  #include <unistd.h>
29  #include <string.h>
30  #define _stricmp(x,y) strcasecmp(x,y)
31 #endif
32 
33 #define PLUGIN_VERSION 1
34 
35 LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind
36 
37 
38 #define CONCAT(x,y,z) x y z
39 #define strConCat(x,y,z) CONCAT(x,y,z)
40 #define LUA_GET_SENSOR_DATA_COMMAND "simExtSkeleton_getSensorData"
41 #define LUA_KUKA_LBR_IIWA_START_COMMAND "simExtKukaLBRiiwaStart"
42 
43 
44 
45 std::shared_ptr<grl::vrep::KukaVrepPlugin> kukaPluginPG;
46 
48  16, // Example Value // Parameter name
49  sim_lua_arg_string|sim_lua_arg_table,0, // joint handle table with 0 or more strings
50  sim_lua_arg_string,0, // "RobotFlangeTip" , // RobotFlangeTipHandle,
51  sim_lua_arg_string,0, // "RobotMillTip" , // RobotTipHandle,
52  sim_lua_arg_string,0, // "RobotMillTipTarget" , // RobotTargetHandle,
53  sim_lua_arg_string,0, // "Robotiiwa" , // RobotTargetBaseHandle, aka RobotName
54  sim_lua_arg_string,0, // "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800)
55  sim_lua_arg_string,0, // "tcp://0.0.0.0:30010" , // LocalUDPAddress
56  sim_lua_arg_string,0, // "30010" , // LocalUDPPort
57  sim_lua_arg_string,0, // "tcp://172.31.1.147:30010", // RemoteUDPAddress
58  sim_lua_arg_string,0, // "192.170.10.100" , // LocalHostKukaKoniUDPAddress,
59  sim_lua_arg_string,0, // "30200" , // LocalHostKukaKoniUDPPort,
60  sim_lua_arg_string,0, // "192.170.10.2" , // RemoteHostKukaKoniUDPAddress,
61  sim_lua_arg_string,0, // "30200" , // RemoteHostKukaKoniUDPPort
62  sim_lua_arg_string,0, // "JAVA" , // KukaCommandMode (options are "JAVA", "FRI")
63  sim_lua_arg_string,0, // "FRI" , // KukaMonitorMode (options are "JAVA", "FRI")
64  sim_lua_arg_string,0, // "IK_Group1_iiwa" // IKGroupName (VREP built in inverse kinematics group)
65 };
66 
67 std::string LUA_KUKA_LBR_IIWA_START_CALL_TIP("number result=simExtKukaLBRiiwaStart(string_table JointHandles , string RobotTipHandle, string RobotFlangeTipHandle, string RobotTargetHandle, string RobotTargetBaseHandle, string RobotModel, string LocalUDPAddress, string LocalUDPPort, string RemoteUDPAddress, string LocalHostKukaKoniUDPAddress, string LocalHostKukaKoniUDPPort, string RemoteHostKukaKoniUDPAddress, string RemoteHostKukaKoniUDPPort, string KukaCommandMode, string KukaMonitorMode, string IKGroupName) -- KukaCommandMode options are JAVA and FRI");
68 
70 { // the callback function of the new Lua command ("simExtSkeleton_getSensorData")
71  // return Lua Table or arrays containing position, torque, torque minus motor force, timestamp, FRI state
72 
73  try {
74  if (!kukaPluginPG) {
75  BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n";
76 
77  CLuaFunctionData data;
78 
80  {
81 
82  std::vector<CLuaFunctionDataItem>* inData=data.getInDataPtr();
83  std::vector<std::string> JointHandles;
84  for (size_t i=0;i<inData->at(0).stringData.size();i++)
85  {
86  JointHandles.push_back(std::string(inData->at(0).stringData[i].c_str()));
87  }
88  std::string RobotFlangeTipHandle (inData->at(1 ).stringData[0]);
89  std::string RobotTipHandle (inData->at(2 ).stringData[0]);
90  std::string RobotTargetHandle (inData->at(3 ).stringData[0]);
91  std::string RobotTargetBaseHandle (inData->at(4 ).stringData[0]);
92  std::string RobotModel (inData->at(5 ).stringData[0]);
93  std::string LocalUDPAddress (inData->at(6 ).stringData[0]);
94  std::string LocalUDPPort (inData->at(7 ).stringData[0]);
95  std::string RemoteUDPAddress (inData->at(8 ).stringData[0]);
96  std::string LocalHostKukaKoniUDPAddress (inData->at(9 ).stringData[0]);
97  std::string LocalHostKukaKoniUDPPort (inData->at(10).stringData[0]);
98  std::string RemoteHostKukaKoniUDPAddress (inData->at(11).stringData[0]);
99  std::string RemoteHostKukaKoniUDPPort (inData->at(12).stringData[0]);
100  std::string KukaCommandMode (inData->at(13).stringData[0]);
101  std::string KukaMonitorMode (inData->at(14).stringData[0]);
102  std::string IKGroupName (inData->at(15).stringData[0]);
103 
104 
105  kukaPluginPG=std::make_shared<grl::vrep::KukaVrepPlugin>(
106  std::make_tuple(
107  JointHandles ,
108  RobotFlangeTipHandle ,
109  RobotTipHandle ,
110  RobotTargetHandle ,
111  RobotTargetBaseHandle ,
112  RobotModel ,
113  LocalUDPAddress ,
114  LocalUDPPort ,
115  RemoteUDPAddress ,
116  LocalHostKukaKoniUDPAddress ,
117  LocalHostKukaKoniUDPPort ,
118  RemoteHostKukaKoniUDPAddress ,
119  RemoteHostKukaKoniUDPPort ,
120  KukaCommandMode ,
121  KukaMonitorMode ,
122  IKGroupName
123  )
124  );
125  kukaPluginPG->construct();
126  } else {
127  /// @todo report an error?
128  // use default params
129  kukaPluginPG=std::make_shared<grl::vrep::KukaVrepPlugin>();
130  kukaPluginPG->construct();
131  }
132 
133 
134 
135 
136  }
137 
138  } catch (const boost::exception& e){
139  // log the error and print it to the screen, don't release the exception
140  std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e));
141  simAddStatusbarMessage( initerr.c_str());
142  BOOST_LOG_TRIVIAL(error) << initerr;
143  kukaPluginPG.reset();
144  } catch (const std::exception& e){
145  // log the error and print it to the screen, don't release the exception
146  std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e));
147  simAddStatusbarMessage( initerr.c_str());
148  BOOST_LOG_TRIVIAL(error) << initerr;
149  kukaPluginPG.reset();
150  } catch (...){
151  // log the error and print it to the screen, don't release the exception
152  std::string initerr("v_repExtKukaLBRiiwa plugin encountered an unknown error and will disable itself. Please debug this issue! file and line:" + std::string(__FILE__) + " " + boost::lexical_cast<std::string>(__LINE__) + "\n");
153  simAddStatusbarMessage( initerr.c_str());
154  BOOST_LOG_TRIVIAL(error) << initerr;
155  kukaPluginPG.reset();
156  }
157 }
158 
159 
160 // This is the plugin start routine (called just once, just after the plugin was loaded):
161 VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
162 {
163  // Dynamically load and bind V-REP functions:
164  // ******************************************
165  // 1. Figure out this plugin's directory:
166  char curDirAndFile[1024];
167 #ifdef _WIN32
168  GetModuleFileName(NULL,curDirAndFile,1023);
169  PathRemoveFileSpec(curDirAndFile);
170 #elif defined (__linux) || defined (__APPLE__)
171  getcwd(curDirAndFile, sizeof(curDirAndFile));
172 #endif
173  std::string currentDirAndPath(curDirAndFile);
174  // 2. Append the V-REP library's name:
175  std::string temp(currentDirAndPath);
176 #ifdef _WIN32
177  temp+="\\v_rep.dll";
178 #elif defined (__linux)
179  temp+="/libv_rep.so";
180 #elif defined (__APPLE__)
181  temp+="/libv_rep.dylib";
182 #endif /* __linux || __APPLE__ */
183  // 3. Load the V-REP library:
184  vrepLib=loadVrepLibrary(temp.c_str());
185  if (vrepLib==NULL)
186  {
187  BOOST_LOG_TRIVIAL(error) << "Error, could not find or correctly load the V-REP library. Cannot start 'v_repExtKukaLBRiiwa' plugin.\n";
188  return(0); // Means error, V-REP will unload this plugin
189  }
191  {
192  BOOST_LOG_TRIVIAL(error) << "Error, could not find all required functions in the V-REP library. Cannot start 'v_repExtKukaLBRiiwa' plugin.\n";
194  return(0); // Means error, V-REP will unload this plugin
195  }
196  // ******************************************
197 
198  // Check the version of V-REP:
199  // ******************************************
200  int vrepVer;
202  if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
203  {
204  BOOST_LOG_TRIVIAL(error) << "Sorry, your V-REP copy is somewhat old. Cannot start 'v_repExtKukaLBRiiwa' plugin.\n";
206  return(0); // Means error, V-REP will unload this plugin
207  }
208  // ******************************************
209 
210 
211  // Register the new Lua command "simExtSkeleton_getSensorData":
212  // ******************************************
213 
214  std::vector<int> inArgs;
215 
218  (
221  &inArgs[0],
223  );
224 
225 
226 
227  // Expected input arguments are: int sensorIndex, float floatParameters[3], int intParameters[2]
228  //int inArgs_getSensorData[]={3,sim_lua_arg_int,sim_lua_arg_float|sim_lua_arg_table,sim_lua_arg_int|sim_lua_arg_table}; // this says we expect 3 arguments (1 integer, a table of floats, and a table of ints)
229  // Return value can change on the fly, so no need to specify them here, except for the calltip.
230  // Now register the callback:
231  //simRegisterCustomLuaFunction(LUA_GET_SENSOR_DATA_COMMAND,strConCat("number result,table data,number distance=",LUA_GET_SENSOR_DATA_COMMAND,"(number sensorIndex,table_3 floatParams,table_2 intParams)"),inArgs_getSensorData,LUA_GET_SENSOR_DATA_CALLBACK);
232  // ******************************************
233 
234  BOOST_LOG_TRIVIAL(info) << "KUKA LBR iiwa plugin initialized. Build date/time: " << __DATE__ << " " << __TIME__ <<"\n";
235 
236  return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
237 }
238 
239 // This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
240 VREP_DLLEXPORT void v_repEnd()
241 {
242  // Here you could handle various clean-up tasks
243 
244  /////////////////////////
245  // PUT OBJECT RESET CODE HERE
246  // close out as necessary
247  ////////////////////
248 
249  kukaPluginPG.reset();
250 
251  unloadVrepLibrary(vrepLib); // release the library
252 }
253 
254 // This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
255 VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
256 { // This is called quite often. Just watch out for messages/events you want to handle
257  // Keep following 5 lines at the beginning and unchanged:
258  static bool refreshDlgFlag=true;
259  int errorModeSaved;
262  void* retVal=NULL;
263 
264  // Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here.
265  // For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
266  // in the V-REP user manual.
267 
268 
270  refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
271 
273  { // A custom menu bar entry was selected..
274  // here you could make a plugin's main dialog visible/invisible
275  }
276 
278  { // This message is sent each time the scene was rendered (well, shortly after) (very often)
279  // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
280 
281  int flags=auxiliaryData[0];
282  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
283  bool instanceSwitched=((flags&64)!=0);
284 
285  if (instanceSwitched)
286  {
287  // React to an instance switch here!!
288  }
289 
290  if (sceneContentChanged)
291  { // we actualize plugin objects for changes in the scene
292  refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here
293  }
294 
295 
296 
297  //...
298  //////////////
299  // PUT MAIN CODE HERE
300 
301  /////////////
302  if (simGetSimulationState() != sim_simulation_advancing_abouttostop) //checks if the simulation is still running
303  {
304  //if(kukaPluginPG) BOOST_LOG_TRIVIAL(info) << "current simulation time:" << simGetSimulationTime() << std::endl; // gets simulation time point
305  }
306  // make sure it is "right" (what does that mean?)
307 
308 
309  // find the v-rep C functions to do the following:
310  ////////////////////////////////////////////////////
311  // Use handles that were found at the "start" of this simulation running
312 
313  // next few Lines get the joint angles, torque, etc from the simulation
314  if (kukaPluginPG)// && kukaPluginPG->allHandlesSet == true // allHandlesSet now handled internally
315  {
316  try
317  {
318  // run one loop synchronizing the arm and plugin
319  kukaPluginPG->run_one();
320 
321  } catch (const boost::exception& e){
322  // log the error and print it to the screen, don't release the exception
323  std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e));
324  simAddStatusbarMessage( initerr.c_str());
325  BOOST_LOG_TRIVIAL(error) << initerr;
326  kukaPluginPG.reset();
327  } catch (const std::exception& e){
328  // log the error and print it to the screen, don't release the exception
329  std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e));
330  simAddStatusbarMessage( initerr.c_str());
331  BOOST_LOG_TRIVIAL(error) << initerr;
332  kukaPluginPG.reset();
333  } catch (...){
334  // log the error and print it to the screen, don't release the exception
335  std::string initerr("v_repExtKukaLBRiiwa plugin encountered an unknown error and will disable itself. Please debug this issue! file and line:" + std::string(__FILE__) + " " + boost::lexical_cast<std::string>(__LINE__) + "\n");
336  simAddStatusbarMessage( initerr.c_str());
337  BOOST_LOG_TRIVIAL(error) << initerr;
338  kukaPluginPG.reset();
339  }
340 
341  }
342  }
343 
345  { // The main script is about to be run (only called while a simulation is running (and not paused!))
346 
347  }
348 
350  { // Simulation is about to start
351 
352  /////////////////////////
353  // PUT OBJECT STARTUP CODE HERE
354  ////////////////////
355  // get the handles to all the objects, joints, etc that we need
356  /////////////////////
357  // simGetObjectHandle
358 
359 // try {
360 // BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n";
361 // kukaPluginPG = std::make_shared<grl::KukaVrepPlugin>();
362 // kukaPluginPG->construct();
363 // //kukaPluginPG->run_one(); // for debugging purposes only
364 // //kukaPluginPG.reset(); // for debugging purposes only
365 // } catch (boost::exception& e){
366 // // log the error and print it to the screen, don't release the exception
367 // std::string initerr("v_repExtKukaLBRiiwa plugin initialization error:\n" + boost::diagnostic_information(e));
368 // simAddStatusbarMessage( initerr.c_str());
369 // BOOST_LOG_TRIVIAL(error) << initerr;
370 // }
371  }
372 
374  { // Simulation just ended
375 
376  /////////////////////////
377  // PUT OBJECT RESET CODE HERE
378  // close out as necessary
379  ////////////////////
380  BOOST_LOG_TRIVIAL(info) << "Ending KUKA LBR iiwa plugin connection to Kuka iiwa\n";
381  kukaPluginPG.reset();
382 
383  }
384 
386  { // A script called simOpenModule (by default the main script). Is only called during simulation.
387  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
388  {
389  // we arrive here only at the beginning of a simulation
390  }
391  }
392 
394  { // A script called simHandleModule (by default the main script). Is only called during simulation.
395  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
396  {
397  // we arrive here only while a simulation is running
398  }
399  }
400 
402  { // A script called simCloseModule (by default the main script). Is only called during simulation.
403  if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
404  {
405  // we arrive here only at the end of a simulation
406  }
407  }
408 
410  { // Here the user switched the scene. React to this message in a similar way as you would react to a full
411  // scene content change. In this plugin example, we react to an instance switch by reacting to the
412  // sim_message_eventcallback_instancepass message and checking the bit 6 (64) of the auxiliaryData[0]
413  // (see here above)
414 
415  }
416 
418  { // Here we have a plugin that is broadcasting data (the broadcaster will also receive this data!)
419 
420  }
421 
423  { // 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)
424 
425  }
426 
427  // You can add many more messages to handle here
428 
429  if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
430  { // handle refresh of the plugin's dialogs
431  // ...
432  refreshDlgFlag=false;
433  }
434 
435  // Keep following unchanged:
436  simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
437  return(retVal);
438 }
439 
#define PLUGIN_VERSION
int getVrepProcAddresses(LIBRARY lib)
Definition: v_repLib.cpp:672
LIBRARY loadVrepLibrary(const char *pathAndFilename)
Definition: v_repLib.cpp:622
bool readDataFromLua(const SLuaCallBack *p, const int *expectedArguments, int requiredArgumentCount, const char *functionName)
VREP_DLLEXPORT unsigned char v_repStart(void *reservedPointer, int reservedInt)
std::shared_ptr< grl::vrep::KukaVrepPlugin > kukaPluginPG
LIBRARY vrepLib
ptrSimSetIntegerParameter simSetIntegerParameter
Definition: v_repLib.cpp:51
void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack *p)
ptrSimGetIntegerParameter simGetIntegerParameter
Definition: v_repLib.cpp:52
void unloadVrepLibrary(LIBRARY lib)
Definition: v_repLib.cpp:641
VREP_DLLEXPORT void * v_repMessage(int message, int *auxiliaryData, void *customData, int *replyData)
const int inArgs_KUKA_LBR_IIWA_START[]
VREP_DLLEXPORT void v_repEnd()
static void getInputDataForFunctionRegistration(const int *dat, std::vector< int > &outDat)
ptrSimRegisterCustomLuaFunction simRegisterCustomLuaFunction
Definition: v_repLib.cpp:208
#define LUA_KUKA_LBR_IIWA_START_COMMAND
std::string LUA_KUKA_LBR_IIWA_START_CALL_TIP("number result=simExtKukaLBRiiwaStart(string_table JointHandles , string RobotTipHandle, string RobotFlangeTipHandle, string RobotTargetHandle, string RobotTargetBaseHandle, string RobotModel, string LocalUDPAddress, string LocalUDPPort, string RemoteUDPAddress, string LocalHostKukaKoniUDPAddress, string LocalHostKukaKoniUDPPort, string RemoteHostKukaKoniUDPAddress, string RemoteHostKukaKoniUDPPort, string KukaCommandMode, string KukaMonitorMode, string IKGroupName) -- KukaCommandMode options are JAVA and FRI")
ptrSimGetSimulationState simGetSimulationState
Definition: v_repLib.cpp:106
std::vector< CLuaFunctionDataItem > * getInDataPtr()
ptrSimAddStatusbarMessage simAddStatusbarMessage
Definition: v_repLib.cpp:117