Torque control example
In the examples subfolder you will find a minimal example for commanding torques to the robot.
It moves the robot in the 5th axis back and forth, while reading the joint positions. To run it
make sure to
have an instance of a robot controller / URSim running at the configured IP address (or adapt the address to your needs)
have PolyScope version 5.23.0 / 10.10.0 or later installed on the robot.
run it from the package’s main folder (the one where the README.md file is stored), as for simplicity reasons it doesn’t use any sophisticated method to locate the required files.
This page will walk you through the full_driver.cpp example.
Initialization
At first, we create a ExampleRobotWrapper object giving it the robot’s IP address, script file and RTDE
recipes.
64 bool headless_mode = true;
65 g_my_robot = std::make_unique<urcl::ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
66 "external_control.urp");
67 if (!g_my_robot->isHealthy())
68 {
69 URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
70 return 1;
71 }
72
73 // Torque control requires Software version 5.23 / 10.10 or higher. Error and exit on older
74 // software versions.
75 {
76 auto robot_version = g_my_robot->getUrDriver()->getVersion();
77 if (robot_version < urcl::VersionInformation::fromString("5.23.0") ||
78 (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0")))
79 {
80 URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.",
81 robot_version.toString().c_str());
82 return 0;
83 }
84 }
85 // --------------- INITIALIZATION END -------------------
Note
This example requires PolyScope version 5.23.0 / 10.10.0 or later, as it uses the direct_torque_control mode which is only available in these versions and later. If you try to run it on an older software version, this example will print an error and exit.
Robot control loop
This example reads the robot’s joint positions, commands a torque for the 5th axis and sends that back as a joint command for the next cycle. This way, the robot will move its wrist first until a positive limit and then back to 0.
To read the joint data, the driver’s RTDE client is used:
94 // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
95 // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
96 // loop.
97 g_my_robot->getUrDriver()->startRTDECommunication();
98 auto start_time = std::chrono::system_clock::now();
99 while (!(passed_positive_part && passed_negative_part))
100 {
101 // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
102 // robot will effectively be in charge of setting the frequency of this loop.
103 // In a real-world application this thread should be scheduled with real-time priority in order
104 // to ensure that this is called in time.
105 std::unique_ptr<urcl::rtde_interface::DataPackage> data_pkg = g_my_robot->getUrDriver()->getDataPackage();
106 if (!data_pkg)
107 {
108 URCL_LOG_WARN("Could not get fresh data package from robot");
109 return 1;
110 }
111 // Read current joint positions from robot data
112 if (!data_pkg->getData("actual_q", g_joint_positions))
113 {
114 // This throwing should never happen unless misconfigured
115 std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
116 throw std::runtime_error(error_msg);
117 }
The first read operation will initialize the target buffer with the current robot position. Next, the target joint torques are set based on the current joint positions:
119 // Open loop control. The target is incremented with a constant each control loop
120 if (passed_positive_part == false)
121 {
122 if (g_joint_positions[JOINT_INDEX] >= 2)
123 {
124 passed_positive_part = true;
125 cmd_torque = -torque_abs;
126 }
127 }
128 else if (passed_negative_part == false)
129 {
130 if (g_joint_positions[JOINT_INDEX] <= 0)
131 {
132 cmd_torque = torque_abs;
133 passed_negative_part = true;
134 }
135 }
136 target_torques[JOINT_INDEX] = cmd_torque;
To send the control command, the robot’s ReverseInterface is used via the
writeJointCommand() function:
138 // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more
139 // reliable on non-realtime systems. Use with caution in productive applications. Having it
140 // this high means that the robot will continue a motion for this given time if no new command
141 // is sent / the connection is interrupted.
142 bool ret = g_my_robot->getUrDriver()->writeJointCommand(target_torques, urcl::comm::ControlMode::MODE_TORQUE,
143 urcl::RobotReceiveTimeout::millisec(100));
144 if (!ret)
145 {
146 URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
147 return 1;
148 }