UCLV Dynamixel Utils
|
The Hand class represents a robotic hand with multiple motors. More...
#include <hand.hpp>
Public Member Functions | |
Hand (const std::string &serial_port, int baudrate, float protocol_version, dynamixel::PortHandler *const &portHandler, dynamixel::PacketHandler *const &packetHandler) | |
Constructs a Hand object. | |
Hand (const std::string &serial_port, int baudrate, float protocol_version) | |
Constructs a Hand object. | |
Hand () | |
Default constructor. | |
~Hand () | |
Destructor. | |
bool | initialize () |
Initializes the hand. | |
void | setSerialPort (const std::string &serial_port) |
Sets the serial port. | |
void | setBaudrate (int baudrate) |
Sets the baudrate. | |
void | setProtocolVersion (float protocol_version) |
Sets the protocol version. | |
std::string | getSerialPort () |
Gets the serial port. | |
int | getBaudrate () |
Gets the baudrate. | |
float | getProtocolVersion () |
Gets the protocol version. | |
void | setPortHandler (dynamixel::PortHandler *portHandler) |
Sets the port handler. | |
void | setPacketHandler (dynamixel::PacketHandler *packetHandler) |
Sets the packet handler. | |
void | setSerialPortLowLatency (const std::string &serial_port) |
Sets the serial port to low latency mode. | |
std::shared_ptr< FingerMotor > | createFingerMotor (uint8_t id) |
Creates a finger motor. | |
std::shared_ptr< WristMotor > | createWristMotor (uint8_t id) |
Creates a wrist motor. | |
const std::vector< std::shared_ptr< FingerMotor > > & | addFingerMotor (uint8_t id) |
Adds a finger motor to the hand. | |
const std::vector< std::shared_ptr< WristMotor > > & | addWristMotor (uint8_t id) |
Adds a wrist motor to the hand. | |
std::vector< std::shared_ptr< FingerMotor > > | getFingerMotors () |
Gets the finger motors. | |
std::vector< std::shared_ptr< WristMotor > > | getWristMotors () |
Gets the wrist motors. | |
void | printFingerMotors () const |
Prints the finger motors. | |
void | printWristMotors () const |
Prints the wrist motors. | |
void | removeFingerMotor (uint8_t id) |
Removes a finger motor from the hand. | |
void | removeWristMotor (uint8_t id) |
Removes a wrist motor from the hand. | |
void | moveFingerMotor (const uint8_t &id, const float &position) |
Moves a finger motor to a specified position. | |
void | moveWristMotor (const uint8_t &id, const float &position) |
Moves a wrist motor to a specified position. | |
uint16_t | readFingerMotorPosition (const uint8_t &id) |
Reads the position of a finger motor. | |
uint16_t | readWristMotorPosition (const uint8_t &id) |
Reads the position of a wrist motor. | |
void | moveMotors (const std::vector< uint8_t > &ids, const std::vector< float > &positions) |
Moves multiple motors using bulk write. | |
std::vector< uint32_t > | readMotorsPositions (const std::vector< uint8_t > &ids) |
Reads the positions of multiple motors using bulk read. | |
The Hand class represents a robotic hand with multiple motors.
Hand::Hand | ( | const std::string & | serial_port, |
int | baudrate, | ||
float | protocol_version, | ||
dynamixel::PortHandler *const & | portHandler, | ||
dynamixel::PacketHandler *const & | packetHandler ) |
Constructs a Hand object.
Constructs a Hand object with the specified parameters.
serial_port | The serial port to which the hand is connected. |
baudrate | The baudrate for the serial communication. |
protocol_version | The protocol version used for communication. |
portHandler | Pointer to the port handler. |
packetHandler | Pointer to the packet handler. |
serial_port | The serial port to be used for communication. |
baudrate | The baudrate for serial communication. |
protocol_version | The protocol version to be used. |
portHandler | Pointer to the port handler. |
packetHandler | Pointer to the packet handler. |
Hand::Hand | ( | const std::string & | serial_port, |
int | baudrate, | ||
float | protocol_version ) |
Constructs a Hand object.
Constructs a Hand object with the specified parameters and initializes the port and packet handlers.
serial_port | The serial port to which the hand is connected. |
baudrate | The baudrate for the serial communication. |
protocol_version | The protocol version used for communication. |
serial_port | The serial port to be used for communication. |
baudrate | The baudrate for serial communication. |
protocol_version | The protocol version to be used. |
Hand::Hand | ( | ) |
Default constructor.
Default constructor for Hand.
Hand::~Hand | ( | ) |
Destructor.
Destructor for Hand. Cleans up the port and packet handlers.
const std::vector< std::shared_ptr< FingerMotor > > & Hand::addFingerMotor | ( | uint8_t | id | ) |
Adds a finger motor to the hand.
Adds a finger motor to the list if it does not already exist.
id | The identifier of the motor. |
This function checks if a FingerMotor with the specified ID already exists in the list. If it does not, a new FingerMotor is created and added to the list.
id | The ID of the FingerMotor to be added. |
const std::vector< std::shared_ptr< WristMotor > > & Hand::addWristMotor | ( | uint8_t | id | ) |
Adds a wrist motor to the hand.
Adds a wrist motor to the list if it does not already exist.
id | The identifier of the motor. |
This function checks if a WristMotor with the specified ID already exists in the list. If it does not, a new WristMotor is created and added to the list.
id | The ID of the WristMotor to be added. |
std::shared_ptr< FingerMotor > Hand::createFingerMotor | ( | uint8_t | id | ) |
Creates a finger motor.
Creates a finger motor with the specified ID.
id | The identifier of the motor. |
id | The ID of the finger motor. |
std::shared_ptr< WristMotor > Hand::createWristMotor | ( | uint8_t | id | ) |
Creates a wrist motor.
Creates a wrist motor with the specified ID.
id | The identifier of the motor. |
id | The ID of the wrist motor. |
int Hand::getBaudrate | ( | ) |
Gets the baudrate.
std::vector< std::shared_ptr< FingerMotor > > Hand::getFingerMotors | ( | ) |
Gets the finger motors.
Gets the list of finger motors.
float Hand::getProtocolVersion | ( | ) |
Gets the protocol version.
std::string Hand::getSerialPort | ( | ) |
Gets the serial port.
std::vector< std::shared_ptr< WristMotor > > Hand::getWristMotors | ( | ) |
Gets the wrist motors.
Gets the list of wrist motors.
bool Hand::initialize | ( | ) |
Initializes the hand.
Initializes the serial port and sets the baudrate.
void Hand::moveFingerMotor | ( | const uint8_t & | id, |
const float & | position ) |
Moves a finger motor to a specified position.
id | The identifier of the motor. |
position | The target position. |
This function sets the target position for a FingerMotor identified by its ID. The position value should be within the range of 0 to 4095.
id | The ID of the FingerMotor to be moved. |
position | The target position for the FingerMotor. It should be a value between 0 and 4095. |
void Hand::moveMotors | ( | const std::vector< uint8_t > & | ids, |
const std::vector< float > & | positions ) |
Moves multiple motors using bulk write.
Moves multiple motors to specified target positions using bulk write.
ids | A vector of motor identifiers. |
positions | A vector of target positions. |
This function moves a set of motors (both finger and wrist motors) to specified positions in a single bulk write operation.
ids | A vector of motor IDs to be moved. |
positions | A vector of target positions corresponding to each motor ID. Values should be in the range 0 to 4095. |
void Hand::moveWristMotor | ( | const uint8_t & | id, |
const float & | position ) |
Moves a wrist motor to a specified position.
id | The identifier of the motor. |
position | The target position. |
This function sets the target position for a WristMotor identified by its ID. The position value should be within the range of 0 to 4095.
id | The ID of the WristMotor to be moved. |
position | The target position for the WristMotor. It should be a value between 0 and 4095. |
void Hand::printFingerMotors | ( | ) | const |
Prints the finger motors.
Prints the IDs of all finger motors.
This function prints the index and ID of each finger motor in the list. If no finger motors are found, it prints a message indicating this.
void Hand::printWristMotors | ( | ) | const |
Prints the wrist motors.
Prints the IDs of all wrist motors.
This function prints the index and ID of each wrist motor in the list. If no wrist motors are found, it prints a message indicating this.
uint16_t Hand::readFingerMotorPosition | ( | const uint8_t & | id | ) |
Reads the position of a finger motor.
Reads the current position of a specified finger motor.
id | The identifier of the motor. |
This function retrieves the current position of a FingerMotor identified by its ID.
id | The ID of the FingerMotor whose position is to be read. |
std::vector< uint32_t > Hand::readMotorsPositions | ( | const std::vector< uint8_t > & | ids | ) |
Reads the positions of multiple motors using bulk read.
ids | A vector of motor identifiers. |
This function reads the current positions of a set of motors (both finger and wrist motors) in a single bulk read operation.
ids | A vector of motor IDs whose positions are to be read. |
uint16_t Hand::readWristMotorPosition | ( | const uint8_t & | id | ) |
Reads the position of a wrist motor.
Reads the current position of a specified wrist motor.
id | The identifier of the motor. |
This function retrieves the current position of a WristMotor identified by its ID.
id | The ID of the WristMotor whose position is to be read. |
void Hand::removeFingerMotor | ( | uint8_t | id | ) |
Removes a finger motor from the hand.
Removes a finger motor from the list by its ID.
id | The identifier of the motor to be removed. |
This function searches for a finger motor with the specified ID and removes it from the list of finger motors if found.
id | The ID of the finger motor to remove. |
void Hand::removeWristMotor | ( | uint8_t | id | ) |
Removes a wrist motor from the hand.
Removes a wrist motor from the list by its ID.
id | The identifier of the motor to be removed. |
This function searches for a wrist motor with the specified ID and removes it from the list of wrist motors if found.
id | The ID of the wrist motor to remove. |
void Hand::setBaudrate | ( | int | baudrate | ) |
Sets the baudrate.
baudrate | The baudrate for the serial communication. |
baudrate | The baudrate for serial communication. |
void Hand::setPacketHandler | ( | dynamixel::PacketHandler * | packetHandler | ) |
Sets the packet handler.
packetHandler | Pointer to the packet handler. |
void Hand::setPortHandler | ( | dynamixel::PortHandler * | portHandler | ) |
Sets the port handler.
portHandler | Pointer to the port handler. |
void Hand::setProtocolVersion | ( | float | protocol_version | ) |
Sets the protocol version.
protocol_version | The protocol version used for communication. |
protocol_version | The protocol version to be used. |
void Hand::setSerialPort | ( | const std::string & | serial_port | ) |
Sets the serial port.
serial_port | The serial port to which the hand is connected. |
serial_port | The serial port to be used for communication. |
void Hand::setSerialPortLowLatency | ( | const std::string & | serial_port | ) |
Sets the serial port to low latency mode.
serial_port | The serial port to which the hand is connected. |
serial_port | The serial port to be used. |