46 #include <visp3/core/vpTime.h> 47 #include <visp3/robot/vpVirtuose.h> 49 #if defined(VISP_HAVE_VIRTUOSE) 51 void CallBackVirtuose(VirtContext VC,
void* ptr)
55 static bool firstIteration =
true;
63 int force_increase_rate = 500;
64 float cube_size = 0.05f;
72 double virtualStiffnessAng = 20;
73 double virtualDamperAng = 0.182;
74 double virtualDamperAng2 = 0.0456;
103 if (firstIteration) {
104 localPosition0 = localPosition;
105 firstIteration =
false;
166 rotzYZ = zeed.skew(zeed)*zYZ.normalize();
167 vpColVector forceStiff1 = virtualStiffnessAng*rotzYZ;
170 for (
unsigned int i=0; i<3; i++)
171 torque1[i] = forceStiff1[i] - forceDamp1[i];
176 rotzXZ = zeed.skew(zeed)*zXZ.normalize();
177 vpColVector forceStiff2 = virtualStiffnessAng*rotzXZ;
180 for (
unsigned int i=0; i<3; i++)
181 torque2[i] = forceStiff2[i] - forceDamp2[i];
191 rotxXY = xdd.skew(xdd)*xXY.normalize();
192 alpha = asin(rotxXY[2]);
194 vpColVector forceStiff3 = virtualStiffnessAng*alpha*zdd;
195 vpColVector forceDamp3 = virtualDamperAng2*(omegad*zdd)*zdd;
196 for (
unsigned int i=0; i<3; i++)
197 torque3[i] = forceStiff3[i] - forceDamp3[i];
199 for (
unsigned int j=0; j<3; j++)
200 forceEe[j+3] = torque1[j] + torque2[j]+torque3[j];
202 forceEe = dFMb * forceEe;
208 for (
unsigned int i=0; i<3; i++) {
209 p_min[i] = localPosition0[i] - cube_size / 2;
210 p_max[i] = localPosition0[i] + cube_size / 2;
213 for (
int i=0; i < 3; i++) {
214 if ((p_min[i] >= localPosition[i]))
216 forceFeedback[i] = (p_min[i] - localPosition[i]) * force_increase_rate;
217 if (forceFeedback[i] >= force_limit) forceFeedback[i] = force_limit;
219 else if ((p_max[i] <= localPosition[i]))
221 forceFeedback[i] = (p_max[i] - localPosition[i]) * force_increase_rate;
222 if (forceFeedback[i] <= -force_limit) forceFeedback[i] = -force_limit;
225 forceFeedback[i] = 0;
228 for (
unsigned int j=0; j<6; j++)
229 finalForce[j] = forceFeedback[j] + forceEe[j];
260 std::cout <<
"The end" << std::endl;
270 std::cout <<
"You should install pthread and/or Virtuose API to use this binary..." << std::endl;
void startPeriodicFunction()
void extract(vpRotationMatrix &R) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpPoseVector getPhysicalPosition() const
error that can be emited by ViSP classes.
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
void setForce(const vpColVector &force)
vpRotationMatrix inverse() const
Implementation of a rotation matrix and operations on such kind of matrices.
vpColVector & normalize()
void setVerbose(bool mode)
VISP_EXPORT void sleepMs(double t)
vpColVector getCol(const unsigned int j) const
Implementation of a velocity twist matrix and operations on such kind of matrices.
void setIpAddress(const std::string &ip)
Implementation of a force/torque twist matrix and operations on such kind of matrices.
void stopPeriodicFunction()
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpColVector getPhysicalVelocity() const
Class that consider the case of a translation vector.
const std::string & getStringMessage(void) const
Send a reference (constant) related the error message (can be empty).