RobotPololuPtu

class RobotPololuPtu(self: visp._visp.robot.RobotPololuPtu, device: str = /dev/ttyACM0, baudrate: int = 9600, verbose: bool = false)

Bases: Robot

Interface for the Pololu Maestro pan-tilt unit using two servo motors.

See https://www.pololu.com/category/102/maestro-usb-servo-controllers for more details.

This class handle the vpPololu class in a higher level and allows to control the pan-tilt unit using position or velocity commands.

The corresponding Denavit-Hartenberg representations of the PTU is the following:

<unparsed table <doxmlparser.compound.docTableType object at 0x7f121e600820>>

Default constructor.

Parameters:
device

Name of the serial interface used for communication.

baudrate

Baudrate used for the serial communication. Note that this parameter is only used on Windows.

verbose

When true, enable verbose mode.

Methods

__init__

Default constructor.

getAngularVelocityResolution

Return the minimul angular velocity in rad/s that could be applied to move the motors.

getPosition

Overloaded function.

getPositioningVelocityPercentage

Get the percentage of the maximum velocity applied to move the PTU in position.

get_eJe

Overloaded function.

get_fJe

Overloaded function.

setPosition

setPositioningVelocityPercentage

Set the percentage of the maximum velocity applied to move the PTU in position.

setRobotState

Overloaded function.

setVelocity

setVerbose

Overloaded function.

stopVelocity

Stop the velocity command.

Inherited Methods

STATE_VELOCITY_CONTROL

MIXT_FRAME

getNDof

Return robot degrees of freedom number.

RobotStateType

Robot control frames.

REFERENCE_FRAME

STATE_POSITION_CONTROL

ControlFrameType

Robot control frames.

STATE_ACCELERATION_CONTROL

CAMERA_FRAME

setMaxRotationVelocity

Set the maximal rotation velocity that can be sent to the robot during a velocity control.

getMaxTranslationVelocity

Get the maximal translation velocity that can be sent to the robot during a velocity control.

TOOL_FRAME

JOINT_STATE

STATE_STOP

END_EFFECTOR_FRAME

ARTICULAR_FRAME

saturateVelocities

Saturate velocities.

setMaxTranslationVelocity

Set the maximal translation velocity that can be sent to the robot during a velocity control.

STATE_FORCE_TORQUE_CONTROL

getMaxRotationVelocity

Get the maximal rotation velocity that can be sent to the robot during a velocity control.

getRobotState

Operators

__doc__

__init__

Default constructor.

__module__

Attributes

ARTICULAR_FRAME

CAMERA_FRAME

END_EFFECTOR_FRAME

JOINT_STATE

MIXT_FRAME

REFERENCE_FRAME

STATE_ACCELERATION_CONTROL

STATE_FORCE_TORQUE_CONTROL

STATE_POSITION_CONTROL

STATE_STOP

STATE_VELOCITY_CONTROL

TOOL_FRAME

__annotations__

class ControlFrameType(self, value: int)

Bases: pybind11_object

Robot control frames.

Values:

  • REFERENCE_FRAME: Corresponds to a fixed reference frame attached to the robot structure.

  • ARTICULAR_FRAME: Corresponds to the joint state. This value is deprecated. You should rather use vpRobot::JOINT_STATE .

  • JOINT_STATE: Corresponds to the joint state.

  • END_EFFECTOR_FRAME: Corresponds to robot end-effector frame.

  • CAMERA_FRAME: Corresponds to a frame attached to the camera mounted on the robot end-effector.

  • TOOL_FRAME: Corresponds to a frame attached to the tool (camera, gripper…) mounted on the robot end-effector. This value is equal to vpRobot::CAMERA_FRAME .

  • MIXT_FRAME: Corresponds to a “virtual” frame where translations are expressed in the reference frame, and rotations in the camera frame.

__and__(self, other: object) object
__eq__(self, other: object) bool
__ge__(self, other: object) bool
__getstate__(self) int
__gt__(self, other: object) bool
__hash__(self) int
__index__(self) int
__init__(self, value: int)
__int__(self) int
__invert__(self) object
__le__(self, other: object) bool
__lt__(self, other: object) bool
__ne__(self, other: object) bool
__or__(self, other: object) object
__rand__(self, other: object) object
__ror__(self, other: object) object
__rxor__(self, other: object) object
__setstate__(self, state: int) None
__xor__(self, other: object) object
property name : str
class RobotStateType(self, value: int)

Bases: pybind11_object

Robot control frames.

Values:

  • REFERENCE_FRAME: Corresponds to a fixed reference frame attached to the robot structure.

  • ARTICULAR_FRAME: Corresponds to the joint state. This value is deprecated. You should rather use vpRobot::JOINT_STATE .

  • JOINT_STATE: Corresponds to the joint state.

  • END_EFFECTOR_FRAME: Corresponds to robot end-effector frame.

  • CAMERA_FRAME: Corresponds to a frame attached to the camera mounted on the robot end-effector.

  • TOOL_FRAME: Corresponds to a frame attached to the tool (camera, gripper…) mounted on the robot end-effector. This value is equal to vpRobot::CAMERA_FRAME .

  • MIXT_FRAME: Corresponds to a “virtual” frame where translations are expressed in the reference frame, and rotations in the camera frame.

__and__(self, other: object) object
__eq__(self, other: object) bool
__ge__(self, other: object) bool
__getstate__(self) int
__gt__(self, other: object) bool
__hash__(self) int
__index__(self) int
__init__(self, value: int)
__int__(self) int
__invert__(self) object
__le__(self, other: object) bool
__lt__(self, other: object) bool
__ne__(self, other: object) bool
__or__(self, other: object) object
__rand__(self, other: object) object
__ror__(self, other: object) object
__rxor__(self, other: object) object
__setstate__(self, state: int) None
__xor__(self, other: object) object
property name : str
__init__(self: visp._visp.robot.RobotPololuPtu, device: str = /dev/ttyACM0, baudrate: int = 9600, verbose: bool = false)

Default constructor.

Parameters:
device

Name of the serial interface used for communication.

baudrate

Baudrate used for the serial communication. Note that this parameter is only used on Windows.

verbose

When true, enable verbose mode.

getAngularVelocityResolution(self) float

Return the minimul angular velocity in rad/s that could be applied to move the motors. It corresponds to 1 pwm converted in rad/s.

getMaxRotationVelocity(self) float

Get the maximal rotation velocity that can be sent to the robot during a velocity control.

Returns:

Maximum rotation velocity expressed in rad/s.

getMaxTranslationVelocity(self) float

Get the maximal translation velocity that can be sent to the robot during a velocity control.

Returns:

Maximum translational velocity expressed in m/s.

getNDof(self) int

Return robot degrees of freedom number.

getPosition(*args, **kwargs)

Overloaded function.

  1. getPosition(self: visp._visp.robot.RobotPololuPtu, frame: visp._visp.robot.Robot.ControlFrameType, q: visp._visp.core.ColVector) -> None

  2. getPosition(self: visp._visp.robot.Robot, frame: visp._visp.robot.Robot.ControlFrameType) -> visp._visp.core.ColVector

getPositioningVelocityPercentage(self) float

Get the percentage of the maximum velocity applied to move the PTU in position.

Note

See setPositioningVelocityPercentage()

Returns:

Positioning velocity percentage in [0, 100.0]. The maximum positioning velocity is given vpRobot::getMaxRotationVelocity() .

getRobotState(self) visp._visp.robot.Robot.RobotStateType
get_eJe(*args, **kwargs)

Overloaded function.

  1. get_eJe(self: visp._visp.robot.RobotPololuPtu, eJe: visp._visp.core.Matrix) -> None

Get the robot jacobian expressed in the end-effector frame.

Warning

End-effector frame is not the embedded camera frame. It corresponds to the frame associated to the tilt axis (see also get_cMe).

Parameters:
eJe

Jacobian between end effector frame and end effector frame (on tilt axis).

  1. get_eJe(self: visp._visp.robot.RobotPololuPtu, q: visp._visp.core.ColVector, eJe: visp._visp.core.Matrix) -> None

Get the robot jacobian expressed in the end-effector frame.

Warning

End-effector frame is not the embedded camera frame. It corresponds to the frame associated to the tilt axis (see also get_cMe).

Parameters:
q

Joint positions to consider [rad].

eJe

Jacobian between end effector frame and end effector frame (on tilt axis).

get_fJe(*args, **kwargs)

Overloaded function.

  1. get_fJe(self: visp._visp.robot.RobotPololuPtu, fJe: visp._visp.core.Matrix) -> None

Get the robot jacobian expressed in the robot reference frame.

Parameters:
fJe

Jacobian between reference frame (or fix frame) and end effector frame (on tilt axis).

  1. get_fJe(self: visp._visp.robot.RobotPololuPtu, q: visp._visp.core.ColVector, fJe: visp._visp.core.Matrix) -> None

Get the robot jacobian expressed in the robot reference frame.

Parameters:
q

Joint positions to consider [rad].

fJe

Jacobian between reference frame (or fix frame) and end effector frame (on tilt axis).

static saturateVelocities(v_in: visp._visp.core.ColVector, v_max: visp._visp.core.ColVector, verbose: bool = false) visp._visp.core.ColVector

Saturate velocities.

The code below shows how to use this static method in order to saturate a velocity skew vector.

#include <iostream>

#include <visp3/robot/vpRobot.h>

#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif

int main()
{
  // Set a velocity skew vector
  vpColVector v(6);
  v[0] = 0.1;               // vx in m/s
  v[1] = 0.2;               // vy
  v[2] = 0.3;               // vz
  v[3] = vpMath::rad(10);   // wx in rad/s
  v[4] = vpMath::rad(-10);  // wy
  v[5] = vpMath::rad(20);   // wz

  // Set the maximal allowed velocities
  vpColVector v_max(6);
  for (int i=0; i<3; i++)
    v_max[i] = 0.3;             // in translation (m/s)
  for (int i=3; i<6; i++)
    v_max[i] = vpMath::rad(10); // in rotation (rad/s)

  // Compute the saturated velocity skew vector
  vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);

  std::cout << "v    : " << v.t() << std::endl;
  std::cout << "v max: " << v_max.t() << std::endl;
  std::cout << "v sat: " << v_sat.t() << std::endl;

  return 0;
}
Parameters:
v_in: visp._visp.core.ColVector

Vector of input velocities to saturate. Translation velocities should be expressed in m/s while rotation velocities in rad/s.

v_max: visp._visp.core.ColVector

Vector of maximal allowed velocities. Maximal translation velocities should be expressed in m/s while maximal rotation velocities in rad/s.

verbose: bool = false

Print a message indicating which axis causes the saturation.

Returns:

Saturated velocities.

setMaxRotationVelocity(self, maxVr: float) None

Set the maximal rotation velocity that can be sent to the robot during a velocity control.

setMaxTranslationVelocity(self, maxVt: float) None

Set the maximal translation velocity that can be sent to the robot during a velocity control.

setPosition(self, frame: visp._visp.robot.Robot.ControlFrameType, q: visp._visp.core.ColVector) None
setPositioningVelocityPercentage(self, positioning_velocity_percentage: float) None

Set the percentage of the maximum velocity applied to move the PTU in position.

Note

See getPositioningVelocityPercentage()

Parameters:
positioning_velocity_percentage: float

Percentage between [0,100] of the maximum velocity. The maximum positioning velocity is given vpRobot::getMaxRotationVelocity() .

setRobotState(*args, **kwargs)

Overloaded function.

  1. setRobotState(self: visp._visp.robot.RobotPololuPtu, newState: visp._visp.robot.Robot.RobotStateType) -> visp._visp.robot.Robot.RobotStateType

  2. setRobotState(self: visp._visp.robot.Robot, newState: visp._visp.robot.Robot.RobotStateType) -> visp._visp.robot.Robot.RobotStateType

setVelocity(self, frame: visp._visp.robot.Robot.ControlFrameType, q_dot: visp._visp.core.ColVector) None
setVerbose(*args, **kwargs)

Overloaded function.

  1. setVerbose(self: visp._visp.robot.RobotPololuPtu, verbose: bool) -> None

Enable/disable verbose mode.

Parameters:
verbose

Set to true to enable verbose mode, false otherwise.

  1. setVerbose(self: visp._visp.robot.Robot, verbose: bool) -> None

stopVelocity(self) None

Stop the velocity command.