Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
testVirtuoseHapticBox.cpp

Test Haption Virtuose SDK wrapper to constrain movements inside a cube of pre-determined side. Hard springs are applied to rotations (only translations are allowed).

/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Test for Virtuose SDK wrapper.
*/
#include <visp3/core/vpTime.h>
#include <visp3/robot/vpVirtuose.h>
#if defined(VISP_HAVE_VIRTUOSE)
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
void CallBackVirtuose(VirtContext VC, void *ptr)
{
(void)VC;
static bool firstIteration = true;
static vpPoseVector localPosition0;
vpPoseVector localPosition;
vpColVector forceFeedback(6, 0);
vpColVector finalForce(6, 0);
vpColVector forceEe(6, 0);
int force_limit = 15;
int force_increase_rate = 500;
float cube_size = 0.05f;
// Virtual spring to let the user know where the initial position is
// Estimated Virtuose handle mass = 0.1;
// Estimated Virtuose handle length = 0.23;
// In the ee frame: Virtuose Handle as a cylinder for the inertia
// Estimated Inertia1 = m*l*l/12
// Estimated Inertia2 = m*l*l/2 (rotation w.r.t. e-e z axis)
double virtualStiffnessAng = 20;
double virtualDamperAng = 0.182; // greater than sqrt 4*Inertia1*virtualStiffnessAng
double virtualDamperAng2 = 0.0456; // greater than sqrt 4*Inertia2*virtualStiffnessAng
vpColVector xd(3, 0);
vpColVector yd(3, 0);
vpColVector zd(3, 0);
vpColVector xee(3, 0);
vpColVector zee(3, 0);
vpColVector xeed(3, 0);
vpColVector zeed(3, 0);
vpColVector zYZ(3, 0);
vpColVector zXZ(3, 0);
vpColVector xXY(3, 0);
vpColVector omegad(3, 0);
vpColVector vee(6, 0);
vpColVector veed(6, 0);
double alpha;
vpColVector torque1(3, 0);
vpColVector torque2(3, 0);
vpColVector torque3(3, 0);
vpVirtuose *p_virtuose = (vpVirtuose *)ptr;
localPosition = p_virtuose->getPhysicalPosition();
if (firstIteration) {
localPosition0 = localPosition;
firstIteration = false;
}
// Position and velocity in of the ee expressed in the base frame
pee = localPosition;
vee = p_virtuose->getPhysicalVelocity();
// Z axis = [pee_x pee_y 0]
zd[0] = pee[0];
zd[1] = pee[1];
zd.normalize();
// X axis = [0 0 1]
xd[2] = 1;
// Y axis from cross product
yd = zd.skew(zd) * xd;
// Current orientation of the ee frame
pee.extract(Qee);
pee.extract(tee);
// X and Z axis of the ee frame expressed in the base frame
xee = Qee.getCol(0);
zee = Qee.getCol(2);
// Rotation matrix from Desired Frame to Base Frame
Qd[0][0] = xd[0];
Qd[1][0] = xd[1];
Qd[2][0] = xd[2];
Qd[0][1] = yd[0];
Qd[1][1] = yd[1];
Qd[2][1] = yd[2];
Qd[0][2] = zd[0];
Qd[1][2] = zd[1];
Qd[2][2] = zd[2];
// X and Z axis of the ee frame expressed in the desired frame
xeed = Qd.inverse() * xee;
zeed = Qd.inverse() * zee;
vpHomogeneousMatrix dMb(tee, Qd);
// Velocity twist matrix for expressing velocities in the desired frame
// Force twist matrix for expressing forces in the base frame
vpForceTwistMatrix dFMb(dMb);
veed = dVMb * vee;
// Angular velocity in the desired frame
omegad[0] = veed[3];
omegad[1] = veed[4];
omegad[2] = veed[5];
// Projection of Z axis of the ee frame onto plane YZ (expressed in the
// desired frame)
zYZ[1] = zeed[1];
zYZ[2] = zeed[2];
// Projection of Z axis of the ee frame onto plane XZ (expressed in the
// desired frame)
zXZ[0] = zeed[0];
zXZ[2] = zeed[2];
// Hard spring to keep Z axis of the ee frame in the horizontal plane
// Spring applied to the angle between the Z axis of the ee frame and its
// projection in the YZ (horizontal) plane
vpColVector rotzYZ(3, 0);
rotzYZ = zeed.skew(zeed) * zYZ.normalize();
vpColVector forceStiff1 = virtualStiffnessAng * rotzYZ;
vpColVector forceDamp1 = virtualDamperAng * (omegad * rotzYZ.normalize()) * rotzYZ.normalize();
for (unsigned int i = 0; i < 3; i++)
torque1[i] = forceStiff1[i] - forceDamp1[i];
// Hard spring to keep Z axis of the ee frame pointing at the origin
// Spring applied to the angle between the Z axis of the ee frame and its
// projection in the XZ (vertical) plane
vpColVector rotzXZ(3, 0);
rotzXZ = zeed.skew(zeed) * zXZ.normalize();
vpColVector forceStiff2 = virtualStiffnessAng * rotzXZ;
vpColVector forceDamp2 = virtualDamperAng * (omegad * rotzXZ.normalize()) * rotzXZ.normalize();
for (unsigned int i = 0; i < 3; i++)
torque2[i] = forceStiff2[i] - forceDamp2[i];
// Hard spring for rotation around z axis of the ee
xXY[0] = xeed[0];
xXY[1] = xeed[1];
vpColVector xdd(3, 0);
xdd[0] = 1;
vpColVector zdd(3, 0);
zdd[2] = 1;
vpColVector rotxXY(3, 0);
rotxXY = xdd.skew(xdd) * xXY.normalize();
alpha = asin(rotxXY[2]);
vpColVector forceStiff3 = virtualStiffnessAng * alpha * zdd;
vpColVector forceDamp3 = virtualDamperAng2 * (omegad * zdd) * zdd;
for (unsigned int i = 0; i < 3; i++)
torque3[i] = forceStiff3[i] - forceDamp3[i];
for (unsigned int j = 0; j < 3; j++)
forceEe[j + 3] = torque1[j] + torque2[j] + torque3[j];
forceEe = dFMb * forceEe;
// ---------------
// Haptic Box
// ---------------
vpColVector p_min(3, 0), p_max(3, 0);
for (unsigned int i = 0; i < 3; i++) {
p_min[i] = localPosition0[i] - cube_size / 2;
p_max[i] = localPosition0[i] + cube_size / 2;
}
for (int i = 0; i < 3; i++) {
if ((p_min[i] >= localPosition[i])) {
forceFeedback[i] = (p_min[i] - localPosition[i]) * force_increase_rate;
if (forceFeedback[i] >= force_limit)
forceFeedback[i] = force_limit;
}
else if ((p_max[i] <= localPosition[i])) {
forceFeedback[i] = (p_max[i] - localPosition[i]) * force_increase_rate;
if (forceFeedback[i] <= -force_limit)
forceFeedback[i] = -force_limit;
}
else
forceFeedback[i] = 0;
}
for (unsigned int j = 0; j < 6; j++)
finalForce[j] = forceFeedback[j] + forceEe[j];
// Set force feedback
p_virtuose->setForce(finalForce);
return;
}
int main(int argc, char **argv)
{
std::string opt_ip = "localhost";
int opt_port = 5000;
for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--ip")
opt_ip = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--port")
opt_port = std::atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "\nUsage: " << argv[0]
<< " [--ip <localhost>] [--port <port>]"
" [--help] [-h]\n"
<< std::endl
<< "Description: " << std::endl
<< " --ip <localhost>" << std::endl
<< "\tHost IP address. Default value: \"localhost\"." << std::endl
<< std::endl
<< " --port <port>" << std::endl
<< "\tCommunication port. Default value: 5000." << std::endl
<< "\tSuggested values: " << std::endl
<< "\t- 5000 to communicate with the Virtuose." << std::endl
<< "\t- 53210 to communicate with the Virtuose equipped with the Glove." << std::endl
<< std::endl;
return EXIT_SUCCESS;
}
}
try {
vpVirtuose virtuose;
std::cout << "Try to connect to " << opt_ip << " port " << opt_port << std::endl;
virtuose.setIpAddressAndPort(opt_ip, opt_port);
virtuose.setVerbose(true);
virtuose.setPowerOn();
virtuose.setPeriodicFunction(CallBackVirtuose);
int counter = 0;
bool swtch = true;
while (swtch) {
if (counter >= 10) {
virtuose.setPowerOff();
swtch = false;
}
counter++;
}
std::cout << "The end" << std::endl;
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
#else
int main()
{
std::cout << "You should install pthread and/or Virtuose API to use this "
"binary..."
<< std::endl;
return EXIT_SUCCESS;
}
#endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector & normalize()
static vpMatrix skew(const vpColVector &v)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
const std::string & getStringMessage() const
Definition: vpException.cpp:67
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
void extract(vpRotationMatrix &R) const
Implementation of a rotation matrix and operations on such kind of matrices.
vpColVector getCol(unsigned int j) const
vpRotationMatrix inverse() const
Class that consider the case of a translation vector.
void setIpAddressAndPort(const std::string &ip, int port)
Definition: vpVirtuose.cpp:81
void setPowerOff()
Definition: vpVirtuose.cpp:925
void setForce(const vpColVector &force)
Definition: vpVirtuose.cpp:749
vpPoseVector getPhysicalPosition() const
Definition: vpVirtuose.cpp:418
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
Definition: vpVirtuose.cpp:885
void stopPeriodicFunction()
void setPowerOn()
Definition: vpVirtuose.cpp:938
void setVerbose(bool mode)
Definition: vpVirtuose.h:196
void startPeriodicFunction()
vpColVector getPhysicalVelocity() const
Definition: vpVirtuose.cpp:452
VISP_EXPORT void sleepMs(double t)