48 #include <visp3/core/vpTime.h> 49 #include <visp3/robot/vpVirtuose.h> 51 #if defined(VISP_HAVE_VIRTUOSE) 53 void CallBackVirtuose(VirtContext VC,
void *ptr)
57 static bool firstIteration =
true;
65 int force_increase_rate = 500;
66 float cube_size = 0.05f;
74 double virtualStiffnessAng = 20;
75 double virtualDamperAng = 0.182;
76 double virtualDamperAng2 = 0.0456;
105 if (firstIteration) {
106 localPosition0 = localPosition;
107 firstIteration =
false;
120 yd = zd.skew(zd) * xd;
171 rotzYZ = zeed.skew(zeed) * zYZ.normalize();
172 vpColVector forceStiff1 = virtualStiffnessAng * rotzYZ;
175 for (
unsigned int i = 0; i < 3; i++)
176 torque1[i] = forceStiff1[i] - forceDamp1[i];
182 rotzXZ = zeed.skew(zeed) * zXZ.normalize();
183 vpColVector forceStiff2 = virtualStiffnessAng * rotzXZ;
186 for (
unsigned int i = 0; i < 3; i++)
187 torque2[i] = forceStiff2[i] - forceDamp2[i];
197 rotxXY = xdd.skew(xdd) * xXY.normalize();
198 alpha = asin(rotxXY[2]);
200 vpColVector forceStiff3 = virtualStiffnessAng * alpha * zdd;
201 vpColVector forceDamp3 = virtualDamperAng2 * (omegad * zdd) * zdd;
202 for (
unsigned int i = 0; i < 3; i++)
203 torque3[i] = forceStiff3[i] - forceDamp3[i];
205 for (
unsigned int j = 0; j < 3; j++)
206 forceEe[j + 3] = torque1[j] + torque2[j] + torque3[j];
208 forceEe = dFMb * forceEe;
214 for (
unsigned int i = 0; i < 3; i++) {
215 p_min[i] = localPosition0[i] - cube_size / 2;
216 p_max[i] = localPosition0[i] + cube_size / 2;
219 for (
int i = 0; i < 3; i++) {
220 if ((p_min[i] >= localPosition[i])) {
221 forceFeedback[i] = (p_min[i] - localPosition[i]) * force_increase_rate;
222 if (forceFeedback[i] >= force_limit)
223 forceFeedback[i] = force_limit;
224 }
else if ((p_max[i] <= localPosition[i])) {
225 forceFeedback[i] = (p_max[i] - localPosition[i]) * force_increase_rate;
226 if (forceFeedback[i] <= -force_limit)
227 forceFeedback[i] = -force_limit;
229 forceFeedback[i] = 0;
232 for (
unsigned int j = 0; j < 6; j++)
233 finalForce[j] = forceFeedback[j] + forceEe[j];
241 int main(
int argc,
char **argv)
243 std::string opt_ip =
"localhost";
245 for (
int i = 0; i < argc; i++) {
246 if (std::string(argv[i]) ==
"--ip")
247 opt_ip = std::string(argv[i + 1]);
248 else if (std::string(argv[i]) ==
"--port")
249 opt_port = std::atoi(argv[i + 1]);
250 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
251 std::cout <<
"\nUsage: " << argv[0]
252 <<
" [--ip <localhost>] [--port <port>]" 255 <<
"Description: " << std::endl
256 <<
" --ip <localhost>" << std::endl
257 <<
"\tHost IP address. Default value: \"localhost\"." << std::endl
259 <<
" --port <port>" << std::endl
260 <<
"\tCommunication port. Default value: 5000." << std::endl
261 <<
"\tSuggested values: " << std::endl
262 <<
"\t- 5000 to communicate with the Virtuose." << std::endl
263 <<
"\t- 53210 to communicate with the Virtuose equipped with the Glove." << std::endl
271 std::cout <<
"Try to connect to " << opt_ip <<
" port " << opt_port << std::endl;
290 std::cout <<
"The end" << std::endl;
299 std::cout <<
"You should install pthread and/or Virtuose API to use this " void startPeriodicFunction()
vpRotationMatrix inverse() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
void setForce(const vpColVector &force)
vpColVector getCol(unsigned int j) const
Implementation of a rotation matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
void setIpAddressAndPort(const std::string &ip, int port)
vpColVector getPhysicalVelocity() const
vpColVector & normalize()
void setVerbose(bool mode)
VISP_EXPORT void sleepMs(double t)
void stopPeriodicFunction()
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpPoseVector getPhysicalPosition() const
const std::string & getStringMessage() const
Send a reference (constant) related the error message (can be empty).
Class that consider the case of a translation vector.