Visual Servoing Platform  version 3.3.0
testVirtuoseJointLimits.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Test for Virtuose SDK wrapper.
33  *
34  * Authors:
35  * Nicolò Pedemonte
36  *
37  *****************************************************************************/
38 
47 #include <visp3/core/vpTime.h>
48 #include <visp3/robot/vpVirtuose.h>
49 
50 #if defined(VISP_HAVE_VIRTUOSE)
51 
52 void CallBackVirtuose(VirtContext VC, void *ptr)
53 {
54  (void)VC;
55  vpVirtuose *p_virtuose = (vpVirtuose *)ptr;
56 
57  float maxQ[6] = {0.7811045051f, -0.07668215036f, 2.481732368f, 2.819076777f, 1.044736624f, 2.687076807f};
58  float minQ[6] = {-0.8011951447f, -1.648244739f, 0.7439950705f, -3.022218227f, -1.260564089f, -2.054088593f};
59  unsigned int numJoint = 6;
60 
61  vpColVector feedbackRegion(numJoint, 0);
62  vpColVector forceFeedback(numJoint, 0);
63 
64  int feedbackRegionFactor = 10;
65  float saturationForce[6] = {5, 5, 5, 2.5, 2.5, 2.5};
66 
67  for (unsigned int iter = 0; iter < numJoint; iter++)
68  feedbackRegion[iter] = (maxQ[iter] - minQ[iter]) / feedbackRegionFactor;
69 
70  vpColVector currentQ = p_virtuose->getArticularPosition();
71 
72  // force feedback definition
73  for (unsigned int iter = 0; iter < numJoint; iter++) {
74  if (currentQ[iter] >= (maxQ[iter] - feedbackRegion[iter])) {
75  forceFeedback[iter] =
76  -saturationForce[iter] * pow((currentQ[iter] - maxQ[iter] + feedbackRegion[iter]) / feedbackRegion[iter], 2);
77  std::cout << "WARNING! Getting close to the maximum joint limit. Joint #" << iter + 1 << std::endl;
78  } else if (currentQ[iter] <= (minQ[iter] + feedbackRegion[iter])) {
79  forceFeedback[iter] =
80  saturationForce[iter] * pow((minQ[iter] + feedbackRegion[iter] - currentQ[iter]) / feedbackRegion[iter], 2);
81  std::cout << "WARNING! Getting close to the minimum joint limit. Joint #" << iter + 1 << std::endl;
82  } else {
83  forceFeedback[iter] = 0;
84  std::cout << "Safe zone" << std::endl;
85  }
86  }
87 
88  // Printing force feedback
89  // std::cout << "Force feedback: " << forceFeedback.t() << std::endl;
90 
91  // Set force feedback
92  p_virtuose->setArticularForce(forceFeedback);
93 
94  return;
95 }
96 
97 int main()
98 {
99  try {
100  float period = 0.001f;
101  vpVirtuose virtuose;
102  virtuose.setTimeStep(period);
103  virtuose.setIpAddress("localhost#53210");
104  virtuose.setVerbose(true);
105  virtuose.setPowerOn();
106 
107  // setArticularForce only works in COMMAND_TYPE_ARTICULAR_IMPEDANCE.
108  virtuose.setCommandType(COMMAND_TYPE_ARTICULAR_IMPEDANCE);
109 
110  // -----------------------------------------------------------
111  // Code to obtain (experimentally) the Virtuose joint limits
112  // -----------------------------------------------------------
113 
114  /*
115  // Move the Virtuose in all its workspace while running this code
116 
117  vpColVector joints(6);
118  vpColVector max_joint(6,-1000);
119  vpColVector min_joint(6,1000);
120 
121  for(unsigned int iter=0; iter<10000; iter++) {
122  virtuose.getArticularPosition(joints);
123  for(unsigned int i=0; i<6; i++) {
124  if (joints[i] > max_joint[i])
125  max_joint[i] = joints[i];
126  if (joints[i] < min_joint[i])
127  min_joint[i] = joints[i];
128  }
129  // Printing joint values
130  std::cout << "Joint values: " << joints.t() << std::endl;
131  vpTime::wait(10);
132  }
133 
134  std::cout << "Max Joint values: " << max_joint.t() << std::endl;
135  std::cout << "Min Joint values: " << min_joint.t() << std::endl;
136 
137  // Best Result (small errors are to be expected)
138  // Max Joint values: 0.7811045051 -0.07668215036 2.481732368
139  2.819076777 1.044736624 2.687076807
140  // Min Joint values: -0.8011951447 -1.648244739 0.7439950705
141  -3.022218227 -1.260564089 -2.054088593
142 */
143 
144  virtuose.setPeriodicFunction(CallBackVirtuose);
145  virtuose.startPeriodicFunction();
146 
147  int counter = 0;
148  bool swtch = true;
149 
150  while (swtch) {
151  if (counter >= 10) {
152  virtuose.stopPeriodicFunction();
153  virtuose.setPowerOff();
154  swtch = false;
155  }
156  counter++;
157  vpTime::sleepMs(1000);
158  }
159 
160  std::cout << "The end" << std::endl;
161  } catch (const vpException &e) {
162  std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
163  }
164 }
165 
166 #else
167 int main() { std::cout << "You should install Virtuose API to use this binary..." << std::endl; }
168 #endif
vpException::getStringMessage
const std::string & getStringMessage(void) const
Send a reference (constant) related the error message (can be empty).
Definition: vpException.cpp:91
vpVirtuose::stopPeriodicFunction
void stopPeriodicFunction()
Definition: vpVirtuose.cpp:1017
vpVirtuose::setPowerOn
void setPowerOn()
Definition: vpVirtuose.cpp:913
vpVirtuose::setVerbose
void setVerbose(bool mode)
Definition: vpVirtuose.h:195
vpVirtuose::setCommandType
void setCommandType(const VirtCommandType &type)
Definition: vpVirtuose.cpp:708
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:129
vpVirtuose
Definition: vpVirtuose.h:142
vpVirtuose::setTimeStep
void setTimeStep(const float &timeStep)
Definition: vpVirtuose.cpp:943
vpVirtuose::getArticularPosition
vpColVector getArticularPosition() const
Definition: vpVirtuose.cpp:126
vpVirtuose::setPeriodicFunction
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
Definition: vpVirtuose.cpp:860
vpVirtuose::startPeriodicFunction
void startPeriodicFunction()
Definition: vpVirtuose.cpp:1001
vpVirtuose::setPowerOff
void setPowerOff()
Definition: vpVirtuose.cpp:900
vpTime::sleepMs
VISP_EXPORT void sleepMs(double t)
Definition: vpTime.cpp:270
vpVirtuose::setArticularForce
void setArticularForce(const vpColVector &articularForce)
Definition: vpVirtuose.cpp:588
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpVirtuose::setIpAddress
void setIpAddress(const std::string &ip)
Definition: vpVirtuose.h:181