Class FrankaCartesianPoseInterface

Inheritance Relationships

Base Type

Class Documentation

class FrankaCartesianPoseInterface : public franka_semantic_components::FrankaSemanticComponentInterface

Franka Cartesian Pose interface abstraction on top of hardware_interface to set the full cartesian pose. The Command should either have the form of a 4x4 column major homogenous transformation matrix or a quaternion and translation vector. Optionally, the elbow can be commanded. [joint_3_position, joint_4_sign].

Public Functions

explicit FrankaCartesianPoseInterface(bool command_elbow_activate)

Initializes the franka cartesian velocity interface with access to the hardware interface command interfaces.

Parameters:

command_elbow_active[in] insert true to activate the elbow commanding together with the cartesian velocity input, otherwise the elbow commanding is not allowed.

FrankaCartesianPoseInterface(const FrankaCartesianPoseInterface&) = delete
FrankaCartesianPoseInterface &operator=(const FrankaCartesianPoseInterface &other) = delete
FrankaCartesianPoseInterface &operator=(FrankaCartesianPoseInterface &&other) = delete
FrankaCartesianPoseInterface(FrankaCartesianPoseInterface &&other) = default
~FrankaCartesianPoseInterface() override = default
bool setCommand(const Eigen::Quaterniond &quaternion, const Eigen::Vector3d &translation)

Sets the given orientation and translation command, when elbow is not activated.

Parameters:
  • quaternion[in] rotation represented in quaternion format [x,y,z,w]

  • translation[in] translation represented in Vector3d format [x,y,z]

Returns:

if command was set successfully true, else when elbow is activated false.

bool setCommand(const Eigen::Quaterniond &quaternion, const Eigen::Vector3d &translation, const std::array<double, 2> &elbow_command)

Sets the given command. Based on rotation and translation and the elbow command.

Parameters:
  • quaternion[in] rotation represented in quaternion format [x,y,z,w]

  • translation[in] translation represented in Vector3d format [x,y,z]

  • elbow_command[in] elbow format [joint_3_position, joint_4_sign]

Returns:

true when command was set successfully

Returns:

false when elbow is not activated

bool setCommand(const std::array<double, 16> &pose_command)

Sends the given pose command to the robot.

Parameters:

pose_command[in] column major homogenous transformation matrix.

Returns:

true when command was set successfully

Returns:

false when elbow is activated

bool setCommand(const std::array<double, 16> &pose_command, const std::array<double, 2> &elbow)

Sets the given command.

Parameters:
  • cartesian_pose_command[in] Rotation plus translation commands in column major homogenous transformation matrix.

  • elbow[in] elbow format [joint_3_position, joint_4_sign]

Returns:

true when command was set successfully

Returns:

false when elbow is not activated

std::array<double, 2> getCommandedElbowConfiguration()

Get the commanded elbow interface elbow values.

Throws:

std::runtime_error – if the elbow is not activated.

Returns:

elbow_configuration [joint3_position, joint4_sign]

std::array<double, 16> getCommandedPoseMatrix()

Get the commanded pose interface values.

Returns:

pose_configuration commanded pose values in column major homogenous transformation

std::tuple<Eigen::Quaterniond, Eigen::Vector3d> getCommandedOrientationAndTranslation()

Get the commanded orientation and translation values.

Returns:

std::tuple<Eigen::Quaterniond, Eigen::Vector3d> commanded orientation values in quaternion format. Commanded translation values in Vector3d format [x,y,z].

std::array<double, 2> getCurrentElbowConfiguration()

Get the current elbow configuration.

Throws:

std::runtime_error – if the elbow is not activated.

Returns:

std::array<double, 2> elbow configuration [joint_3_position, joint_4_sign]

std::tuple<Eigen::Quaterniond, Eigen::Vector3d> getCurrentOrientationAndTranslation()

Get the current Orientation And Translation.

Returns:

std::tuple<Eigen::Quaterniond, Eigen::Vector3d> current orientation values in quaternion format. current translation values in Vector3d format [x,y,z].

std::array<double, 16> getCurrentPoseMatrix()

Get the current pose matrix.

Returns:

std::array<double, 16> Current pose matrix column major homogenous transformation