47 #include <visp3/core/vpTime.h>
48 #include <visp3/robot/vpVirtuose.h>
50 #if defined(VISP_HAVE_VIRTUOSE)
52 void CallBackVirtuose(VirtContext VC,
void *ptr)
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;
64 int feedbackRegionFactor = 10;
65 float saturationForce[6] = {5, 5, 5, 2.5, 2.5, 2.5};
67 for (
unsigned int iter = 0; iter < numJoint; iter++)
68 feedbackRegion[iter] = (maxQ[iter] - minQ[iter]) / feedbackRegionFactor;
73 for (
unsigned int iter = 0; iter < numJoint; iter++) {
74 if (currentQ[iter] >= (maxQ[iter] - feedbackRegion[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])) {
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;
83 forceFeedback[iter] = 0;
84 std::cout <<
"Safe zone" << std::endl;
100 float period = 0.001f;
160 std::cout <<
"The end" << std::endl;
167 int main() { std::cout <<
"You should install Virtuose API to use this binary..." << std::endl; }