Basic Usage
In order to use the F/T sensor you need to find this package in your CMakeLists.txt
:
find_package(catkin REQUIRED COMPONENTS
autharl_ati_sensor
)
Then your main should look like this:
#include <autharl_ati_sensor/autharl_ati_sensor.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "node_name");
ros::NodeHandle n;
auto model = std::make_shared<lwr::robot::Model>("/robot_description");
auto robot = std::make_shared<lwr::robot::Robot>(model);
auto sensor = std::make_shared<ati::sensor::Sensor>();
auto rviz = std::make_shared<arl::viz::RosStatePublisher>(robot, sensor);
rviz_thread.join();
return 0;
}
By using the RosStatePublisher thread you will be able to see the wrench measured by the F/T sensor in visualized in RViz. In Rviz add a WrenchStamped panel and use the /ati_sensor
topic. Also if you want to see KUKA LWR and the ATI sensor in the visualization launch one of the URDFs which include the ati sensor. For example, before run the above code launch this file:
roslaunch autharl_description display.launch config:=lwr_ati
You can use the sensor->getData()
function for reading measurements from the sensor.
Tool Mass Compensation
Right now you can compensated values only for the force measurements, not the torque measurements.
Follow the following steps in order to use the tool compensation:
- Plug in the ATI Sensor without the tool mounted.
- Compensate the bias of the sensor by its interface at: 192.168.2.1.
- Mount your tool.
- Use constructor ati::sensor::Sensor(double m, const std::string& name = "Ati Sensor", const std::string& frame_id = "ati_sensor") to initialize the Sensor by providing the tool mass.
- Use the Eigen::Vector6d getData(const Eigen::Matrix3d& rotation) function for reading data.
Examples
Examples exist in core/examples
directory.
Notes
Please for the safety of the hardware do the following:
- Be carefull of the cable of the sensor along the robot. During the motion of the robot the cable should not be stressed.
- Unplug the sensor when you finish your experiments.