YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoard_nws_ros2_callbacks.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
8#endif
9
11
12#include <chrono>
13#include <functional>
14#include <memory>
15#include <string>
16#include <cmath>
17
19#include <yarp/os/LogStream.h>
20#include <Ros2Utils.h>
21#include <rcutils/logging_macros.h>
22
23using namespace std::chrono_literals;
24using namespace yarp::os;
25using namespace yarp::dev;
26using namespace yarp::sig;
27
28namespace {
29YARP_LOG_COMPONENT(CONTROLBOARD_ROS2, "yarp.ros2.controlBoard_nws_ros2", yarp::os::Log::TraceType);
30
31// UTILITIES ----------------------------------------------------------------------------------------------------------- START //
32
33// Maps for quick control mode conversion
34
35const std::map<yarp::conf::vocab32_t,std::string> fromCtrlModeToString{{VOCAB_CM_IDLE,"IDLE"},
36 {VOCAB_CM_TORQUE,"TORQUE"},
37 {VOCAB_CM_POSITION,"POSITION"},
38 {VOCAB_CM_POSITION_DIRECT,"POSITION_DIRECT"},
39 {VOCAB_CM_VELOCITY,"VELOCITY"},
40 {VOCAB_CM_CURRENT,"CURRENT"},
41 {VOCAB_CM_PWM,"PWM"},
42 {VOCAB_CM_IMPEDANCE_POS,"IMPEDANCE_POS"},
43 {VOCAB_CM_IMPEDANCE_VEL,"IMPEDANCE_VEL"},
44 {VOCAB_CM_MIXED,"MIXED"},
45 {VOCAB_CM_HW_FAULT,"HW_FAULT"},
46 {VOCAB_CM_CALIBRATING,"CALIBRATING"},
47 {VOCAB_CM_CALIB_DONE,"CALIB_DONE"},
48 {VOCAB_CM_NOT_CONFIGURED,"NOT_CONFIGURED"},
49 {VOCAB_CM_CONFIGURED,"CONFIGURED"}
50 };
51const std::map<std::string,yarp::conf::vocab32_t> fromStringToCtrlMode{{"IDLE",VOCAB_CM_IDLE},
52 {"TORQUE",VOCAB_CM_TORQUE},
53 {"POSITION",VOCAB_CM_POSITION},
54 {"POSITION_DIRECT",VOCAB_CM_POSITION_DIRECT},
55 {"VELOCITY",VOCAB_CM_VELOCITY},
56 {"CURRENT",VOCAB_CM_CURRENT},
57 {"PWM",VOCAB_CM_PWM},
58 {"IMPEDANCE_POS",VOCAB_CM_IMPEDANCE_POS},
59 {"IMPEDANCE_VEL",VOCAB_CM_IMPEDANCE_VEL},
60 {"MIXED",VOCAB_CM_MIXED},
61 {"IDLE",VOCAB_CM_IDLE}
62 };
63const std::vector<std::string> implementedCtrlModes{"POSITION",
64 "POSITION_DIRECT",
65 "VELOCITY"};
66
67inline double convertRadiansToDegrees(double degrees)
68{
69 return degrees / M_PI * 180.0;
70}
71
72inline double convertDegreesToRadians(double radians)
73{
74 return radians / 180.0 * M_PI;
75}
76
77} // namespace
78
79// UTILITIES ------------------------------------------------------------------------------------------------------------- END //
80
81
82bool ControlBoard_nws_ros2::messageVectorsCheck(const std::string &valueName, const std::vector<std::string> &names, const std::vector<double> &ref_values, const std::vector<double> &derivative){
83 if(!namesCheck(names)) {
84 return false;
85 }
86
87 bool allJoints = names.size() == 0 || names.size() == m_subdevice_joints;
88
89 if(ref_values.size() == 0){
90 yCError(CONTROLBOARD_ROS2) << "The" << valueName << "vector cannot be empty";
91 RCLCPP_ERROR(m_node->get_logger(),"%s vector cannot be empty",valueName.c_str());
92
93 return false;
94 }
95 if(!allJoints && (names.size() != ref_values.size())){
96 yCError(CONTROLBOARD_ROS2) << "The" << valueName << "vector and the names one are not the same size";
97 RCLCPP_ERROR(m_node->get_logger(),"The %s vector and the names one are not the same size",valueName.c_str());
98
99 return false;
100 }
101
102 bool noRef = derivative.size() == 0;
103 if(!noRef && (ref_values.size() != derivative.size())){
104 yCError(CONTROLBOARD_ROS2) << "The" << valueName << "vector and the secondary one are not the same size";
105 RCLCPP_ERROR(m_node->get_logger(),"The %s vector and the secondary one are not the same size",valueName.c_str());
106
107 return false;
108 }
109 bool allJointsFail = allJoints && (ref_values.size() != m_subdevice_joints || (!noRef && derivative.size() != m_subdevice_joints));
110
111 if(allJointsFail){
112 yCError(CONTROLBOARD_ROS2) << "All joints where selected bt the vector sizes do not match";
113 RCLCPP_ERROR(m_node->get_logger(),"All joints where selected bt the vector sizes do not match");
114
115 return false;
116 }
117
118 return true;
119}
120
121
122bool ControlBoard_nws_ros2::messageVectorsCheck(const std::string &valueName, const std::vector<std::string> &names, const std::vector<double> &ref_values){
123 if(!namesCheck(names)) {
124 return false;
125 }
126
127 bool allJoints = names.size() == 0 || names.size() == m_subdevice_joints;
128 bool diffValNames = !allJoints && (names.size() != ref_values.size());
129 bool emptyValues = ref_values.size() == 0;
130
131 if(emptyValues){
132 yCError(CONTROLBOARD_ROS2) << "The" << valueName << "vector cannot be empty";
133 RCLCPP_ERROR(m_node->get_logger(),"The %s vector cannot be empty",valueName.c_str());
134
135 return false;
136 }
137
138 bool allJointsFail = allJoints && ref_values.size() != m_subdevice_joints;
139
140 return !diffValNames && !allJointsFail;
141}
142
143
144bool ControlBoard_nws_ros2::namesCheck(const std::vector<std::string> &names){
145 if(names.size() > m_subdevice_joints){
146 yCError(CONTROLBOARD_ROS2) << "The specified joint names vector is longer than expected";
147 RCLCPP_ERROR(m_node->get_logger(),"The specified joint names vector is longer than expected");
148
149 return false;
150 }
151 for (const auto& name : names){
152 if(m_quickJointRef.count(name) == 0){
153 yCError(CONTROLBOARD_ROS2) << name << "is not a valid joint name";
154 RCLCPP_ERROR(m_node->get_logger(),"%s is not a valid joint name",name.c_str());
155
156 return false;
157 }
158 }
159
160 return true;
161}
162
163
164bool ControlBoard_nws_ros2::messageVectorsCheck(const std::string &valueName, const std::vector<std::string> &names, const std::vector<std::string> &ref_values){
165 if(!namesCheck(names)) {
166 return false;
167 }
168
169 bool allJoints = names.size() == 0 || names.size() == m_subdevice_joints;
170 bool diffValNames = !allJoints && (names.size() != ref_values.size());
171 bool emptyValues = ref_values.size() == 0;
172
173 if(emptyValues){
174 yCError(CONTROLBOARD_ROS2) << "The" << valueName << "vector cannot be empty";
175 RCLCPP_ERROR(m_node->get_logger(),"The %s vector cannot be empty",valueName.c_str());
176
177 return false;
178 }
179
180 bool allJointsFail = allJoints && ref_values.size() != m_subdevice_joints;
181
182 return !diffValNames && !allJointsFail;
183}
184
185
186void ControlBoard_nws_ros2::positionTopic_callback(const yarp_control_msgs::msg::Position::SharedPtr msg) {
187
188 std::lock_guard <std::mutex> lg(m_cmdMutex);
189
190 bool noJoints = msg->names.size() == 0;
191 bool noSpeed = msg->ref_velocities.size() == 0;
192
193 if(!msg){
194 yCError(CONTROLBOARD_ROS2) << "Invalid message";
195 RCLCPP_ERROR(m_node->get_logger(),"Invalid message");
196
197 return;
198 }
199 if(!messageVectorsCheck("Position",msg->names,msg->positions,msg->ref_velocities)){
200
201 return;
202 }
203
204 double tempVel;
205 double tempPos;
207 std::vector<double> convertedPos;
208 std::vector<int> selectedJoints;
209 std::vector<double> convertedVel;
210
211 for(size_t i=0; i<(noJoints ? m_subdevice_joints : msg->positions.size()); i++){
212 size_t index = noJoints ? i : m_quickJointRef[msg->names[i]];
213 if(!noJoints) {selectedJoints.push_back(index);}
214 m_iAxisInfo->getJointType(index, jType);
215 if(!noSpeed){
217 tempVel = convertRadiansToDegrees(msg->ref_velocities[i]);
218 }
219 else{
220 tempVel = msg->ref_velocities[i];
221 }
222 convertedVel.push_back(tempVel);
223 }
224
226 tempPos = convertRadiansToDegrees(msg->positions[i]);
227 }
228 else{
229 tempPos = msg->positions[i];
230 }
231 convertedPos.push_back(tempPos);
232 }
233 if(noJoints){
234 if(!noSpeed){
235 m_iPositionControl->setRefSpeeds(&convertedVel[0]);
236 }
237 m_iPositionControl->positionMove(&convertedPos[0]);
238 }
239 else{
240 if(!noSpeed){
241 m_iPositionControl->setRefSpeeds(convertedPos.size(),&selectedJoints[0],&convertedVel[0]);
242 }
243 m_iPositionControl->positionMove(convertedPos.size(),&selectedJoints[0],&convertedPos[0]);
244 }
245}
246
247
248void ControlBoard_nws_ros2::positionDirectTopic_callback(const yarp_control_msgs::msg::PositionDirect::SharedPtr msg) {
249 std::lock_guard <std::mutex> lg(m_cmdMutex);
250
251 bool noJoints = msg->names.size() == 0;
252
253 if(!msg){
254 yCError(CONTROLBOARD_ROS2) << "Invalid message";
255 RCLCPP_ERROR(m_node->get_logger(),"Invalid message");
256
257 return;
258 }
259
260 if(!messageVectorsCheck("Position",msg->names,msg->positions)){
261
262 return;
263 }
264
265 double tempPos;
267 std::vector<double> convertedPos;
268 std::vector<int> selectedJoints;
269
270 for(size_t i=0; i<(noJoints ? m_subdevice_joints : msg->positions.size()); i++){
271 size_t index = noJoints ? i : m_quickJointRef[msg->names[i]];
272 if(!noJoints) {selectedJoints.push_back(index);}
273 m_iAxisInfo->getJointType(index, jType);
275 tempPos = convertRadiansToDegrees(msg->positions[i]);
276 }
277 else{
278 tempPos = msg->positions[i];
279 }
280 convertedPos.push_back(tempPos);
281 }
282
283 if(noJoints){
284 m_iPositionDirect->setPositions(&convertedPos[0]);
285 }
286 else{
287 m_iPositionDirect->setPositions(convertedPos.size(),&selectedJoints[0],&convertedPos[0]);
288 }
289}
290
291
292void ControlBoard_nws_ros2::velocityTopic_callback(const yarp_control_msgs::msg::Velocity::SharedPtr msg) {
293
294 std::lock_guard <std::mutex> lg(m_cmdMutex);
295
296 bool noJoints = msg->names.size() == 0;
297 bool noAccel = msg->ref_accelerations.size() == 0;
298
299 if(!msg){
300 yCError(CONTROLBOARD_ROS2) << "Invalid message";
301 RCLCPP_ERROR(m_node->get_logger(),"Invalid message");
302
303 return;
304 }
305 if(!messageVectorsCheck("Velocities",msg->names,msg->velocities,msg->ref_accelerations)){
306
307 return;
308 }
309
310 double tempVel;
311 double tempAccel;
313 std::vector<double> convertedVel;
314 std::vector<int> selectedJoints;
315 std::vector<double> convertedAccel;
316
317 for(size_t i=0; i<(noJoints ? m_subdevice_joints : msg->velocities.size()); i++){
318 size_t index = noJoints ? i : m_quickJointRef[msg->names[i]];
319 if(!noJoints) {selectedJoints.push_back(index);}
320 m_iAxisInfo->getJointType(index, jType);
321 if(!noAccel){
323 tempAccel = convertRadiansToDegrees(msg->ref_accelerations[i]);
324 }
325 else{
326 tempAccel = msg->ref_accelerations[i];
327 }
328 convertedAccel.push_back(tempAccel);
329 }
331 tempVel = convertRadiansToDegrees(msg->velocities[i]);
332 }
333 else{
334 tempVel = msg->velocities[i];
335 }
336 convertedVel.push_back(tempVel);
337 }
338 if(noJoints){
339 if(!noAccel){
340 m_iVelocityControl->setRefAccelerations(&convertedAccel[0]);
341 }
342 m_iVelocityControl->velocityMove(&convertedVel[0]);
343 }
344 else{
345 if(!noAccel){
346 m_iVelocityControl->setRefAccelerations(convertedAccel.size(),&selectedJoints[0],&convertedAccel[0]);
347 }
348 m_iVelocityControl->velocityMove(convertedVel.size(),&selectedJoints[0],&convertedVel[0]);
349 }
350}
351
352
353void ControlBoard_nws_ros2::getJointsNamesCallback(const std::shared_ptr<rmw_request_id_t> request_header,
354 const std::shared_ptr<yarp_control_msgs::srv::GetJointsNames::Request> request,
355 std::shared_ptr<yarp_control_msgs::srv::GetJointsNames::Response> response){
356 std::lock_guard <std::mutex> lg(m_cmdMutex);
357 if(!request){
358 yCError(CONTROLBOARD_ROS2) << "Invalid request";
359 RCLCPP_ERROR(m_node->get_logger(),"Invalid request");
360
361 response->response = "INVALID";
362
363 return;
364 }
365
366 bool noIndexes = request->joint_indexes.size() == 0;
367
368 if(!noIndexes && request->joint_indexes.size() > m_subdevice_joints){
369 yCError(CONTROLBOARD_ROS2) << "request->joint_indexes vector cannot be longer than the actual number of joints:"<< request->joint_indexes.size() << "instead of" << m_subdevice_joints;
370 RCLCPP_ERROR(m_node->get_logger(),"request->joint_indexes vector cannot be longer than the actual number of joints: %ld instead of %ld", request->joint_indexes.size(), m_subdevice_joints);
371
372 response->response = "SIZE_ERROR";
373
374 return;
375 }
376
377 std::string tempName;
378 if(noIndexes){
379 for(size_t i=0; i<m_subdevice_joints; i++){
380 if(!m_iAxisInfo->getAxisName(i,tempName)){
381 yCError(CONTROLBOARD_ROS2) << "Name retrieval failed for joint number"<<i;
382 RCLCPP_ERROR(m_node->get_logger(),"Name retrieval failed for joint number %ld",i);
383
384 response->response = "GET_NAME_ERROR";
385
386 return;
387 }
388 response->names.push_back(tempName);
389 }
390 }
391 else{
392 for(const auto &i : request->joint_indexes){
393 if(!m_iAxisInfo->getAxisName(i,tempName)){
394 yCError(CONTROLBOARD_ROS2) << "Name retrieval failed for joint number"<<i;
395 RCLCPP_ERROR(m_node->get_logger(),"Name retrieval failed for joint number %d",i);
396
397 response->response = "GET_NAME_ERROR";
398
399 return;
400 }
401 response->names.push_back(tempName);
402 }
403 }
404
405 response->response = "OK";
406}
407
408
409void ControlBoard_nws_ros2::getAvailableModesCallback(const std::shared_ptr<rmw_request_id_t> request_header,
410 const std::shared_ptr<yarp_control_msgs::srv::GetAvailableControlModes::Request> request,
411 std::shared_ptr<yarp_control_msgs::srv::GetAvailableControlModes::Response> response){
412 std::lock_guard <std::mutex> lg(m_cmdMutex);
413 if(!request){
414 yCError(CONTROLBOARD_ROS2) << "Invalid request";
415 RCLCPP_ERROR(m_node->get_logger(),"Invalid request");
416
417 response->response = "INVALID";
418
419 return;
420 }
421
422 if(request->only_implemented){
423 response->modes = implementedCtrlModes;
424 }
425 else{
426 for(const auto &x : fromStringToCtrlMode){
427 response->modes.push_back(x.first);
428 }
429 }
430
431 response->response = "OK";
432}
433
434
435void ControlBoard_nws_ros2::getControlModesCallback(const std::shared_ptr<rmw_request_id_t> request_header,
436 const std::shared_ptr<yarp_control_msgs::srv::GetControlModes::Request> request,
437 std::shared_ptr<yarp_control_msgs::srv::GetControlModes::Response> response){
438 std::lock_guard <std::mutex> lg(m_cmdMutex);
439
440 bool noJoints = request->names.size() == 0;
441
442 if(!request){
443 yCError(CONTROLBOARD_ROS2) << "Invalid request";
444 RCLCPP_ERROR(m_node->get_logger(),"Invalid request");
445
446 response->response = "INVALID";
447
448 return;
449 }
450
451 if(!noJoints){
452 if(!namesCheck(request->names)){
453 response->response = "NAMES_ERROR";
454
455 return;
456 }
457 }
458
459 size_t forLimit = noJoints ? m_subdevice_joints : request->names.size();
460 int *tempMode = new int[1];
461 std::vector<std::string> modesToSend;
462
463 for (size_t i=0; i<forLimit; i++){
464
465 if(!m_iControlMode->getControlMode(noJoints ? i : m_quickJointRef[request->names[i]],tempMode)){
466 yCError(CONTROLBOARD_ROS2) << "Error while retrieving the control mode for joint"<<request->names[i];
467 RCLCPP_ERROR(m_node->get_logger(),"Error while retrieving the control mode for joint %s",request->names[i].c_str());
468 response->response = "RETRIEVE_ERROR";
469
470 delete tempMode;
471 return;
472 }
474 }
475 response->modes = modesToSend;
476 response->response = "OK";
477
478 delete tempMode;
479}
480
481
482void ControlBoard_nws_ros2::getPositionCallback(const std::shared_ptr<rmw_request_id_t> request_header,
483 const std::shared_ptr<yarp_control_msgs::srv::GetPosition::Request> request,
484 std::shared_ptr<yarp_control_msgs::srv::GetPosition::Response> response) {
485 std::lock_guard <std::mutex> lg(m_cmdMutex);
486
487 bool noJoints = request->names.size() == 0;
488
489 if(!request){
490 yCError(CONTROLBOARD_ROS2) << "Invalid request";
491 RCLCPP_ERROR(m_node->get_logger(),"Invalid request");
492
493 response->response = "INVALID";
494
495 return;
496 }
497
498 if(!noJoints){
499 if(!namesCheck(request->names)){
500 response->response = "NAMES_ERROR";
501
502 return;
503 }
504 }
505
506 size_t forLimit = noJoints ? m_subdevice_joints : request->names.size();
507 double *tempPos = new double[m_jointNames.size()];
508 std::vector<double> positionsToSend;
509
510 if(!m_iEncodersTimed->getEncoders(tempPos)){
511 yCError(CONTROLBOARD_ROS2) << "Error while retrieving joints positions";
512 RCLCPP_ERROR(m_node->get_logger(),"Error while retrieving joints positions");
513 response->response = "RETRIEVE_ERROR";
514
515 delete tempPos;
516 return;
517 }
518
519 double position;
521 for (size_t i=0; i<forLimit; i++){
522 size_t index = noJoints ? i : m_quickJointRef[request->names[i]];
523 m_iAxisInfo->getJointType(index, jType);
525 {
526 position = convertDegreesToRadians(tempPos[index]);
527 }
528 else
529 {
530 position = tempPos[index];
531 }
532 positionsToSend.push_back(position);
533 }
534 response->positions = positionsToSend;
535 response->response = "OK";
536
537 delete tempPos;
538}
539
540
541void ControlBoard_nws_ros2::getVelocityCallback(const std::shared_ptr<rmw_request_id_t> request_header,
542 const std::shared_ptr<yarp_control_msgs::srv::GetVelocity::Request> request,
543 std::shared_ptr<yarp_control_msgs::srv::GetVelocity::Response> response) {
544 std::lock_guard <std::mutex> lg(m_cmdMutex);
545
546 bool noJoints = request->names.size() == 0;
547
548 if(!request){
549 yCError(CONTROLBOARD_ROS2) << "Invalid request";
550 RCLCPP_ERROR(m_node->get_logger(),"Invalid request");
551
552 response->response = "INVALID";
553
554 return;
555 }
556
557 if(!noJoints){
558 if(!namesCheck(request->names)){
559 response->response = "NAMES_ERROR";
560
561 return;
562 }
563 }
564
565 size_t forLimit = noJoints ? m_subdevice_joints : request->names.size();
566 double *tempVel = new double[m_jointNames.size()];
567 std::vector<double> velocitiesToSend;
568
569 if(!m_iEncodersTimed->getEncoderSpeeds(tempVel)){
570 yCError(CONTROLBOARD_ROS2) << "Error while retrieving joints speeds";
571 RCLCPP_ERROR(m_node->get_logger(),"Error while retrieving joints speeds");
572 response->response = "RETRIEVE_ERROR";
573
574 delete tempVel;
575 return;
576 }
577
578 double velocity;
580 for (size_t i=0; i<forLimit; i++){
581 size_t index = noJoints ? i : m_quickJointRef[request->names[i]];
582 m_iAxisInfo->getJointType(index, jType);
584 {
585 velocity = convertDegreesToRadians(tempVel[index]);
586 }
587 else
588 {
589 velocity = tempVel[index];
590 }
591 velocitiesToSend.push_back(velocity);
592 }
593 response->velocities = velocitiesToSend;
594 response->response = "OK";
595
596 delete tempVel;
597}
598
599
600void ControlBoard_nws_ros2::setControlModesCallback(const std::shared_ptr<rmw_request_id_t> request_header,
601 const std::shared_ptr<yarp_control_msgs::srv::SetControlModes::Request> request,
602 std::shared_ptr<yarp_control_msgs::srv::SetControlModes::Response> response){
603 std::lock_guard <std::mutex> lg(m_cmdMutex);
604
605 bool noJoints = request->names.size() == 0;
606
607 if(!request){
608 yCError(CONTROLBOARD_ROS2) << "Invalid request";
609 RCLCPP_ERROR(m_node->get_logger(),"Invalid request");
610
611 response->response = "INVALID";
612
613 return;
614 }
615
616 if(!messageVectorsCheck("Control mode",request->names,request->modes)){
617
618 response->response = "INVALID";
619
620 return;
621 }
622
623 size_t forLimit = noJoints ? m_subdevice_joints : request->names.size();
624
625 for (size_t i=0; i<forLimit; i++){
626 if(!fromStringToCtrlMode.count(request->modes[i])){
627 yCError(CONTROLBOARD_ROS2) << "Cannot set mode to" << request->modes[i];
628 RCLCPP_ERROR(m_node->get_logger(),"Cannot set mode to %s",request->modes[i].c_str());
629
630 return;
631 }
632 if(!m_iControlMode->setControlMode(noJoints ? i : m_quickJointRef[request->names[i]],fromStringToCtrlMode.at(request->modes[i]))){
633 yCError(CONTROLBOARD_ROS2) << "Error while setting the control mode for joint"<<request->names[i]<<"to"<<request->modes[i];
634 RCLCPP_ERROR(m_node->get_logger(),"Error while setting the control mode for joint %s to %s",request->names[i].c_str(),request->modes[i].c_str());
635 response->response = "SET_ERROR";
636
637 return;
638 }
639 }
640
641 response->response = "OK";
642}
constexpr yarp::conf::vocab32_t VOCAB_CM_MIXED
constexpr yarp::conf::vocab32_t VOCAB_CM_TORQUE
constexpr yarp::conf::vocab32_t VOCAB_CM_PWM
constexpr yarp::conf::vocab32_t VOCAB_CM_HW_FAULT
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_VEL
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
constexpr yarp::conf::vocab32_t VOCAB_CM_CALIB_DONE
constexpr yarp::conf::vocab32_t VOCAB_CM_NOT_CONFIGURED
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
constexpr yarp::conf::vocab32_t VOCAB_CM_CONFIGURED
constexpr yarp::conf::vocab32_t VOCAB_CM_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_POS
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY
constexpr yarp::conf::vocab32_t VOCAB_CM_CALIBRATING
virtual bool getAxisName(int axis, std::string &name)=0
virtual bool getJointType(int axis, yarp::dev::JointTypeEnum &type)
Definition IAxisInfo.h:63
virtual bool setControlMode(const int j, const int mode)=0
Set the current control mode.
virtual bool getControlMode(int j, int *mode)=0
Get the current control mode.
virtual bool getEncoders(double *encs)=0
Read the position of all axes.
virtual bool getEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all axes.
virtual bool setRefSpeeds(const double *spds)=0
Set reference speed on all joints.
virtual bool positionMove(int j, double ref)=0
Set new reference point for a single axis.
virtual bool setPositions(const int n_joint, const int *joints, const double *refs)=0
Set new reference point for all axes.
virtual bool setRefAccelerations(const double *accs)=0
Set reference acceleration on all joints.
virtual bool velocityMove(int j, double sp)=0
Start motion at a given speed, single joint.
A mini-server for performing network communication in the background.
@ TraceType
Definition Log.h:92
#define M_PI
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
@ VOCAB_JOINTTYPE_REVOLUTE
Definition IAxisInfo.h:24
An interface to the operating system, including Port based communication.
double convertDegreesToRadians(double degrees)
convert degrees to radiants for ROS messages