Class Robot

Class Documentation

class Robot

Public Functions

explicit Robot(std::unique_ptr<franka::Robot> robot, std::unique_ptr<Model> model)

Construct a new Robot object for tests purposes.

Parameters:
  • robot – mocked libfranka robot

  • model – mocked model object

explicit Robot(const std::string &robot_ip, const rclcpp::Logger &logger)

Connects to the robot. This method can block for up to one minute if the robot is not responding. An exception will be thrown if the connection cannot be established.

Parameters:
  • robot_ip[in] IP address or hostname of the robot.

  • [im] – logger ROS Logger to print eventual warnings.

Robot(const Robot&) = delete
Robot &operator=(const Robot &other) = delete
Robot &operator=(Robot &&other) = delete
Robot(Robot &&other) = delete
virtual ~Robot()

Stops the currently running loop and closes the connection with the robot.

virtual void initializeTorqueInterface()

Starts the active control for torque control.

virtual void initializeJointVelocityInterface()

Starts the active control for joint velocity control.

virtual void initializeJointPositionInterface()

Starts the active control for joint position control.

virtual void initializeCartesianVelocityInterface()

Starts the active control for cartesian velocity control.

virtual void initializeCartesianPoseInterface()

Starts the active control for cartesian pose control.

virtual void stopRobot()

Stops the continuous communication read with the connected robot.

virtual franka::RobotState readOnce()

Get the current robot state

Returns:

current robot state.

virtual franka_hardware::Model *getModel()

Return pointer to the franka robot model object .

Returns:

pointer to the current robot model.

virtual void writeOnce(const std::array<double, 7> &joint_hardware_command)

This function will automatically propagate the received hardware active command interface

Parameters:

joint_hardware_command[in] joint hardware command either efforts or velocities

virtual void writeOnce(const std::array<double, 6> &cartesian_velocity_command)

Cartesian velocity command

Parameters:

cartesian_velocity_command[in] cartesian level velocity command in format [vx, vy, vz, wx, wy, wz]

virtual void writeOnce(const std::array<double, 6> &cartesian_velocity_command, const std::array<double, 2> &elbow_command)

Cartesian velocity command with elbow command

Parameters:
  • cartesian_velocity_command[in] cartesian level velocity command in format [vx, vy, vz, wx, wy, wz]

  • elbow_command[in] elbow command representing joint3_position in rad and joint4 sign

virtual void writeOnce(const std::array<double, 16> &cartesian_pose_command)

Cartesian pose command

Parameters:

cartesian_pose_command[in] cartesian level pose command in column major format [4x4] homogeneous transformation matrix

virtual void writeOnce(const std::array<double, 16> &cartesian_pose_command, const std::array<double, 2> &elbow_command)

Cartesian pose command with elbow command

Parameters:
  • cartesian_pose_command[in] cartesian level pose command in column major format [4x4] homogeneous transformation matrix

  • elbow_command[in] elbow command representing joint3_position in rad and joint4 sign

virtual void setJointStiffness(const franka_msgs::srv::SetJointStiffness::Request::SharedPtr &req)

Sets the impedance for each joint in the internal controller.

User-provided torques are not affected by this setting.

Parameters:

franka_msgs::srv::SetJointStiffness::Request::SharedPtr[in] requests with JointStiffness values

Throws:
  • CommandException – if the Control reports an error.

  • NetworkException – if the connection is lost, e.g. after a timeout.

virtual void setCartesianStiffness(const franka_msgs::srv::SetCartesianStiffness::Request::SharedPtr &req)

Sets the Cartesian stiffness (for x, y, z, roll, pitch, yaw) in the internal controller.

The values set using Robot::SetCartesianStiffness are used in the direction of the stiffness frame, which can be set with Robot::setK.

Inputs received by the torque controller are not affected by this setting.

Parameters:

franka_msgs::srv::SetCartesianStiffness::Request::SharedPtr[in] request

Throws:
  • CommandException – if the Control reports an error.

  • NetworkException – if the connection is lost, e.g. after a timeout.

virtual void setLoad(const franka_msgs::srv::SetLoad::Request::SharedPtr &req)

Sets dynamic parameters of a payload.

Note

This is not for setting end effector parameters, which have to be set in the administrator’s interface.

Parameters:

franka_msgs::srv::SetLoad::Request::SharedPtr[in] request

Throws:

CommandException – if the Control reports an error.

Throws :

virtual void setTCPFrame(const franka_msgs::srv::SetTCPFrame::Request::SharedPtr &req)

Sets the transformation NETEE from nominal end effector to end effector frame.

The transformation matrix is represented as a vectorized 4x4 matrix in column-major format.

Parameters:

franka_msgs::srv::SetTCPFrame::Request::SharedPtr[in] req

Throws:
  • CommandException – if the Control reports an error.

  • NetworkException – if the connection is lost, e.g. after a timeout.

virtual void setStiffnessFrame(const franka_msgs::srv::SetStiffnessFrame::Request::SharedPtr &req)

Sets the transformation EETK from end effector frame to stiffness frame.

The transformation matrix is represented as a vectorized 4x4 matrix in column-major format.

Parameters:

franka_msgs::srv::SetStiffnessFrame::Request::SharedPtr[in] req.

Throws:
  • CommandException – if the Control reports an error.

  • NetworkException – if the connection is lost, e.g. after a timeout.

virtual void setForceTorqueCollisionBehavior(const franka_msgs::srv::SetForceTorqueCollisionBehavior::Request::SharedPtr &req)

Changes the collision behavior.

Set common torque and force boundaries for acceleration/deceleration and constant velocity movement phases.

Forces or torques between lower and upper threshold are shown as contacts in the RobotState. Forces or torques above the upper threshold are registered as collision and cause the robot to stop moving.

Parameters:

franka_msgs::srv::SetForceTorqueCollisionBehavior::Request::SharedPtr[in] req

Throws:
  • CommandException – if the Control reports an error.

  • NetworkException – if the connection is lost, e.g. after a timeout.

virtual void setFullCollisionBehavior(const franka_msgs::srv::SetFullCollisionBehavior::Request::SharedPtr &req)

Changes the collision behavior.

Set separate torque and force boundaries for acceleration/deceleration and constant velocity movement phases.

Forces or torques between lower and upper threshold are shown as contacts in the RobotState. Forces or torques above the upper threshold are registered as collision and cause the robot to stop moving.

Parameters:

franka_msgs::srv::SetFullCollisionBehavior::Request::SharedPtr[in] request msg

Throws:
  • CommandException – if the Control reports an error.

  • NetworkException – if the connection is lost, e.g. after a timeout.

virtual void automaticErrorRecovery()

Starts an automatic recovery process.

Throws:
  • CommandException – if the Control reports an error.

  • NetworkException – if the connection is lost, e.g. after a timeout.

Protected Functions

Robot() = default