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; }
void startPeriodicFunction()
vpColVector getArticularPosition() const
error that can be emited by ViSP classes.
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
void setVerbose(bool mode)
VISP_EXPORT void sleepMs(double t)
void setCommandType(const VirtCommandType &type)
void setIpAddress(const std::string &ip)
void setArticularForce(const vpColVector &articularForce)
void stopPeriodicFunction()
Implementation of column vector and the associated operations.
void setTimeStep(const float &timeStep)
const std::string & getStringMessage(void) const
Send a reference (constant) related the error message (can be empty).