-
Notifications
You must be signed in to change notification settings - Fork 304
/
cartesian_pose_example_controller.cpp
93 lines (78 loc) · 3.43 KB
/
cartesian_pose_example_controller.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_example_controllers/cartesian_pose_example_controller.h>
#include <cmath>
#include <memory>
#include <stdexcept>
#include <string>
#include <controller_interface/controller_base.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <hardware_interface/hardware_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
namespace franka_example_controllers {
bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& node_handle) {
cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
if (cartesian_pose_interface_ == nullptr) {
ROS_ERROR(
"CartesianPoseExampleController: Could not get Cartesian Pose "
"interface from hardware");
return false;
}
std::string arm_id;
if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
return false;
}
try {
cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
cartesian_pose_interface_->getHandle(arm_id + "_robot"));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
"CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
return false;
}
auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
if (state_interface == nullptr) {
ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware");
return false;
}
try {
auto state_handle = state_interface->getHandle(arm_id + "_robot");
std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
for (size_t i = 0; i < q_start.size(); i++) {
if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
ROS_ERROR_STREAM(
"CartesianPoseExampleController: Robot is not in the expected starting position for "
"running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
"robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
return false;
}
}
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
"CartesianPoseExampleController: Exception getting state handle: " << e.what());
return false;
}
return true;
}
void CartesianPoseExampleController::starting(const ros::Time& /* time */) {
initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
elapsed_time_ = ros::Duration(0.0);
}
void CartesianPoseExampleController::update(const ros::Time& /* time */,
const ros::Duration& period) {
elapsed_time_ += period;
double radius = 0.3;
double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * elapsed_time_.toSec()));
double delta_x = radius * std::sin(angle);
double delta_z = radius * (std::cos(angle) - 1);
std::array<double, 16> new_pose = initial_pose_;
new_pose[12] -= delta_x;
new_pose[14] -= delta_z;
cartesian_pose_handle_->setCommand(new_pose);
}
} // namespace franka_example_controllers
PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianPoseExampleController,
controller_interface::ControllerBase)