39 #include <visp3/core/vpConfig.h>
41 #ifdef VISP_HAVE_TAKKTILE2
43 #include <reflex_driver2.h>
45 #include <visp3/core/vpMath.h>
46 #include <visp3/robot/vpReflexTakktile2.h>
48 #ifndef DOXYGEN_SHOULD_SKIP_THIS
49 class vpReflexTakktile2::Impl :
public reflex_driver2::ReflexDriver
66 for (
size_t i = 0; i < NUM_FINGERS; i++) {
67 pressure[i].resize(NUM_SENSORS_PER_FINGER);
68 contact[i].resize(NUM_SENSORS_PER_FINGER);
74 load.resize(NUM_DYNAMIXELS);
87 for (
size_t i = 0; i < NUM_FINGERS; i++) {
88 os <<
"Finger " << i + 1 <<
": " << std::endl;
90 os <<
"\tProximal: " << hand.
proximal[i] << std::endl;
91 os <<
"\tDistal Approx: " << hand.
distal_approx[i] << std::endl;
93 os <<
"\tPressures: ";
94 for (
size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
100 for (
size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
101 os << hand.
contact[i][j] <<
", ";
105 os <<
"\tJoint Angle: " << hand.
joint_angle[i] <<
" rad" << std::endl;
107 os <<
"\tVelocity: " << hand.
velocity[i] <<
" rad/s" << std::endl;
108 os <<
"\tVelocity: " <<
vpMath::deg(
static_cast<double>(hand.
velocity[i])) <<
" deg/s" << std::endl;
109 os <<
"\tError State: " << hand.
error_state[i] << std::endl;
112 os <<
"Preshape: " << std::endl;
113 os <<
"\tJoint Angle: " << hand.
joint_angle[3] << std::endl;
114 os <<
"\tVelocity: " << hand.
velocity[3] << std::endl;
115 os <<
"\tError State: " << hand.
error_state[3] << std::endl;
149 for (
size_t i = 0; i < NUM_FINGERS; i++) {
152 for (
size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
157 for (
size_t i = 0; i < NUM_DYNAMIXELS; i++) {
192 for (
unsigned int i = 0; i < NUM_SERVOS; i++) {
193 position[i] =
static_cast<double>(m_impl->hand_info.joint_angle[i]);
205 for (
unsigned int i = 0; i < NUM_SERVOS; i++) {
206 velocity[i] =
static_cast<double>(m_impl->hand_info.velocity[i]);
221 if (targets.
size() != NUM_SERVOS) {
223 targets.
size(), NUM_SERVOS);
225 float targets_[NUM_SERVOS];
226 for (
unsigned int i = 0; i < NUM_SERVOS; i++) {
227 targets_[i] =
static_cast<float>(targets[i]);
229 m_impl->set_angle_position(targets_);
249 if (thresholds.size() != NUM_FINGERS * NUM_SENSORS_PER_FINGER) {
251 thresholds.size(), NUM_FINGERS * NUM_SENSORS_PER_FINGER);
253 int thresholds_[NUM_FINGERS * NUM_SENSORS_PER_FINGER];
254 for (
size_t i = 0; i < NUM_FINGERS * NUM_SENSORS_PER_FINGER; i++) {
255 thresholds_[i] = thresholds[i];
258 m_impl->set_tactile_thresholds(thresholds_);
269 if (targets.
size() != NUM_SERVOS) {
271 targets.
size(), NUM_SERVOS);
273 float targets_[NUM_SERVOS];
274 for (
unsigned int i = 0; i < NUM_SERVOS; i++) {
275 targets_[i] =
static_cast<float>(targets[i]);
277 m_impl->set_motor_speed(targets_);
287 if (targets.
size() != NUM_SERVOS) {
289 targets.
size(), NUM_SERVOS);
291 float targets_[NUM_SERVOS];
292 for (
unsigned int i = 0; i < NUM_SERVOS; i++) {
293 targets_[i] =
static_cast<float>(targets[i]);
295 m_impl->move_until_any_contact(targets_);
305 if (targets.
size() != NUM_SERVOS) {
307 targets.
size(), NUM_SERVOS);
309 float targets_[NUM_SERVOS];
310 for (
unsigned int i = 0; i < NUM_SERVOS; i++) {
311 targets_[i] =
static_cast<float>(targets[i]);
313 m_impl->move_until_each_contact(targets_);
322 reflex_hand2::ReflexHandState *state = &m_impl->rh->rx_state_;
323 m_impl->rh->setStateCallback(std::bind(&reflex_driver2::ReflexDriver::reflex_hand_state_cb, m_impl, state));
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
@ dimensionError
Bad dimension.
static double deg(double rad)
std::vector< uint32_t > temperature
std::vector< float > joint_angle
std::vector< std::vector< bool > > contact
std::vector< std::vector< int > > pressure
std::vector< std::string > error_state
std::vector< float > load
std::vector< float > raw_angle
std::vector< float > proximal
std::vector< float > voltage
std::vector< float > distal_approx
std::vector< float > velocity
void setVelocityUntilAnyContact(const vpColVector &targets)
std::string m_finger_file_name
void setVelocityUntilEachContact(const vpColVector &targets)
std::string m_tactile_file_name
void setTactileThreshold(int threshold)
std::string m_network_interface
vpColVector getVelocity() const
void setPosition(const vpColVector &targets)
int getNumFingers() const
void setPositioningVelocity(const vpColVector &targets)
std::string m_motor_file_name
vpColVector getPosition() const
virtual ~vpReflexTakktile2()
int getNumSensorsPerFinger() const
void wait(int milliseconds)