33 : spinner_(1, callback_queue_.
get()),
34 callback_queue_(
boost::make_shared<
ros::CallbackQueue>()),
35 node_handle_control_(
ros::NodeHandle(),
"control") {
49 if (result->error_code != result->SUCCESSFUL) {
50 ROS_WARN_STREAM_NAMED(
"delta_controller",
"Controller [" << controller <<
"] action ended in state [" << state.
toString() <<
"] with error code [" << result->error_code <<
"]");
59 trajectory.second.points.erase(trajectory.second.points.begin(), trajectory.second.points.end()-1);
60 trajectory.second.points.front().time_from_start =
ros::Duration(0.1);
74 for (
int i=0; i<feedback->joint_names.size(); i++) {
75 ROS_DEBUG_STREAM_NAMED(
"delta_controller",
"Controller [" << controller <<
"] joint [" << feedback->joint_names.at(i) <<
"] state is [" << feedback->actual.positions.at(i) <<
"] (expecting [" << feedback->desired.positions.at(i) <<
"]).");
80 visualization_msgs::Marker marker;
81 marker.type = visualization_msgs::Marker::CUBE;
82 marker.scale.x = interactive_marker.scale * 0.4;
83 marker.scale.y = interactive_marker.scale * 0.4;
84 marker.scale.z = interactive_marker.scale * 0.4;
85 marker.color.r = 0.35;
86 marker.color.g = 0.35;
87 marker.color.b = 0.35;
88 marker.color.a = 0.75;
90 visualization_msgs::InteractiveMarkerControl control;
91 control.always_visible =
true;
92 control.markers.push_back(marker);
93 control.name =
"move_xyz";
94 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
95 interactive_marker.controls.push_back(control);
99 visualization_msgs::InteractiveMarkerControl control;
100 control.orientation.w = 1;
101 control.orientation.x = 1;
102 control.orientation.y = 0;
103 control.orientation.z = 0;
104 control.name =
"move_yz";
105 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
106 interactive_marker.controls.push_back(control);
107 control.name =
"move_x";
108 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
109 interactive_marker.controls.push_back(control);
110 control.orientation.w = 1;
111 control.orientation.x = 0;
112 control.orientation.y = 1;
113 control.orientation.z = 0;
114 control.name =
"move_xy";
115 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
116 interactive_marker.controls.push_back(control);
117 control.name =
"move_z";
118 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
119 interactive_marker.controls.push_back(control);
120 control.orientation.w = 1;
121 control.orientation.x = 0;
122 control.orientation.y = 0;
123 control.orientation.z = 1;
124 control.name =
"move_xz";
125 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
126 interactive_marker.controls.push_back(control);
127 control.name =
"move_y";
128 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
129 interactive_marker.controls.push_back(control);
133 geometry_msgs::Point target_pick_pose;
134 target_pick_pose.x = target_pose.x -
ee_offset_.x;
135 target_pick_pose.y = target_pose.y -
ee_offset_.y;
136 target_pick_pose.z = target_pose.z -
ee_offset_.z;
138 std::vector<double> target_joint_positions;
144 for (
auto const &intermediate_pose : intermediate_poses) {
145 std::vector<double> intermediate_joint_positions;
149 joint_positions.push_back(intermediate_joint_positions);
155 return std::sqrt(std::pow(to_pose.x-from_pose.x,2) + std::pow(to_pose.y-from_pose.y,2) + std::pow(to_pose.z-from_pose.z,2));
159 std::vector<geometry_msgs::Point> intermediate_poses;
160 geometry_msgs::Point ee_pose;
165 double displacement_x = (target_pose.x - ee_pose.x)/size;
166 double displacement_y = (target_pose.y - ee_pose.y)/size;
167 double displacement_z = (target_pose.z - ee_pose.z)/size;
168 for (
int i=0; i<size+1; i++) {
169 geometry_msgs::Point pose;
170 pose.x = ee_pose.x + i*displacement_x;
171 pose.y = ee_pose.y + i*displacement_y;
172 pose.z = ee_pose.z + i*displacement_z;
173 intermediate_poses.push_back(pose);
177 return intermediate_poses;
181 std::vector<std::vector<double>> joint_stiffnesses;
184 std::vector<double> displacements;
185 for (
int j=0; j<old_joint_stiffnesses.size(); j++) {
186 displacements.push_back((target_joint_stiffnesses.at(j) - old_joint_stiffnesses.at(j))/size);
189 for (
int i=0; i<size; i++) {
190 std::vector<double> point;
191 for (
int j=0; j<displacements.size(); j++) {
192 point.push_back(old_joint_stiffnesses.at(j) + (i+1)*displacements.at(j));
194 joint_stiffnesses.push_back(point);
197 return joint_stiffnesses;
200 std::map<std::string, trajectory_msgs::JointTrajectory>
DeltaKinematicController::computeJointTrajectories(
const std::vector<std::vector<double>> &motor_positions,
const std::vector<std::vector<double>> &motor_stiffness,
const double &target_time_from_start,
const double &previous_time_from_start) {
201 std::map<std::string, trajectory_msgs::JointTrajectory> trajectories;
202 for (
int id=1;
id<=3;
id++) {
203 trajectory_msgs::JointTrajectory trajectory;
207 for (
int i=0; i<motor_positions.size(); i++) {
208 trajectory_msgs::JointTrajectoryPoint point;
209 point.positions = {motor_positions.at(i).at(
id-1), motor_stiffness.at(i).at(
id-1)};
210 if (i == 0 || i == motor_positions.size()-1) {
211 point.velocities.resize(trajectory.joint_names.size());
212 point.accelerations.resize(trajectory.joint_names.size());
214 point.time_from_start =
ros::Duration(previous_time_from_start + (i+1)*(target_time_from_start-previous_time_from_start)/motor_positions.size());
215 trajectory.points.push_back(point);
217 trajectories.insert(std::make_pair(
getMotorName(
id), trajectory));
223 double R_motors = 0.1;
224 double R_ee = 0.0455;
225 double l_motors = 0.09;
228 double phi_2 = 2*M_PI/3;
229 double phi_3 = 4*M_PI/3;
230 double theta_1 = motor_joints.at(0) - 1;
231 double theta_2 = motor_joints.at(1) - 1;
232 double theta_3 = motor_joints.at(2) - 1;
234 sensor_msgs::JointState msg;
235 std::vector<double> phi = {phi_1, phi_2, phi_3};
236 std::vector<double> theta = {theta_1, theta_2, theta_3};
237 for (
int i=0; i<3; i++) {
238 std::string base_name =
"delta_qbmove_" + std::to_string(i+1);
240 geometry_msgs::Point upperarm_pose;
241 upperarm_pose.x =
cos(phi.at(i))*(l_motors*
cos(theta.at(i))+R_motors);
242 upperarm_pose.y =
sin(phi.at(i))*(l_motors*
cos(theta.at(i))+R_motors);
243 upperarm_pose.z = -l_motors*
sin(theta.at(i));
245 geometry_msgs::Point forearm_pose;
246 forearm_pose.x =
cos(phi.at(i))*R_ee + ee_pose.x;
247 forearm_pose.y =
sin(phi.at(i))*R_ee + ee_pose.y;
248 forearm_pose.z = 0 + ee_pose.z;
250 geometry_msgs::Point diff_pose;
251 diff_pose.x = forearm_pose.x - upperarm_pose.x;
252 diff_pose.y = forearm_pose.y - upperarm_pose.y;
253 diff_pose.z = forearm_pose.z - upperarm_pose.z;
255 geometry_msgs::Point forearm_to_upperarm_vector;
256 forearm_to_upperarm_vector.x = std::cos(-phi.at(i))*diff_pose.x + -std::sin(-phi.at(i))*diff_pose.y;
257 forearm_to_upperarm_vector.y = std::sin(-phi.at(i))*diff_pose.x + std::cos(-phi.at(i))*diff_pose.y;
258 forearm_to_upperarm_vector.z = diff_pose.z;
260 double pitch = -std::atan2(forearm_to_upperarm_vector.z, -forearm_to_upperarm_vector.x);
261 double yaw = std::atan2(forearm_to_upperarm_vector.y, std::hypot(forearm_to_upperarm_vector.x, forearm_to_upperarm_vector.z));
262 msg.name.push_back(base_name +
"_free_down_joint");
263 msg.name.push_back(base_name +
"_free_l_joint");
264 msg.position.push_back(pitch);
265 msg.position.push_back(yaw);
270 geometry_msgs::TransformStamped transform;
271 transform.header.frame_id =
"delta_base_frame_link";
273 transform.child_frame_id =
"delta_ee_frame_link";
274 transform.transform.translation.x = ee_pose.x;
275 transform.transform.translation.y = ee_pose.y;
276 transform.transform.translation.z = ee_pose.z;
277 transform.transform.rotation.w = 1;
283 for (
auto const &trajectory : motor_joint_trajectories) {
293 if (x.size() < b.size() - 1) {
299 if (y.size() < a.size() - 1) {
302 y.push_back((b.front() * x.front()) / a.front());
305 auto it_a = a.begin();
306 auto it_b = b.begin();
307 while (y.size() < a.size() - 1) {
310 for (
auto it_x = x.begin(); it_x != x.begin() + y.size(); it_x++) {
311 bj_xi += *it_x * *(it_b + y.size() - (it_x - x.begin()));
316 for (
auto it_y = y.begin(); it_y != y.end(); it_y++) {
317 aj_yi += *it_y * *(it_a + y.size() - (it_y - y.begin()));
320 y.push_back((bj_xi - aj_yi) / a.front());
324 int num_elements_x_exceed_y = x.size() - y.size();
325 if (num_elements_x_exceed_y < 0) {
329 if (num_elements_x_exceed_y == 0 && y.size() != 1) {
335 for (
auto it_x = x.end() - num_elements_x_exceed_y; it_x != x.end(); it_x++) {
338 for (
auto it_b = b.begin(); it_b != b.end(); it_b++) {
339 bj_xi += *it_b * *(it_x - (it_b - b.begin()));
343 auto it_y = y.begin() + (it_x - x.begin());
346 for (
auto it_a = a.begin() + 1; it_a != a.end(); it_a++) {
347 aj_yi += *it_a * *(it_y - (it_a - a.begin()));
351 y.push_back((bj_xi - aj_yi) / a.front());
358 for (
int id=1;
id<=3;
id++) {
372 std::vector<std::vector<double>> matrix_x(motor_joint_trajectory.joint_names.size(), std::vector<double>());
373 std::vector<std::vector<double>> matrix_y(motor_joint_trajectory.joint_names.size(), std::vector<double>());
374 for (
auto const &point : motor_joint_trajectory.points) {
375 for (
int i=0; i<point.positions.size(); i++) {
376 matrix_x.at(i).push_back(point.positions.at(i));
386 for (
int i=0; i<matrix_x.size(); i++) {
387 matrix_y.at(i).insert(std::end(matrix_y.at(i)), std::begin(matrix_x.at(i)), std::begin(matrix_x.at(i))+
filter_param_a_.size());
388 filter(b, a, matrix_x.at(i), matrix_y.at(i));
391 for (
int j=0; j<matrix_y.at(0).size(); j++) {
392 for (
int i=0; i<motor_joint_trajectory.joint_names.size(); i++) {
393 motor_joint_trajectory.points.at(j).positions.at(i) = matrix_y.at(i).at(j);
405 double R_motors = 0.1;
406 double R_ee = 0.0455;
407 double l_motors = 0.09;
410 double phi_2 = 2*M_PI/3;
411 double phi_3 = 4*M_PI/3;
412 double theta_1 = motor_joints.at(0) - 1;
413 double theta_2 = motor_joints.at(1) - 1;
414 double theta_3 = motor_joints.at(2) - 1;
415 double R = R_motors - R_ee;
432 double x_1 = l_motors*
cos(theta_1) + R;
433 double z_1 = -l_motors*
sin(theta_1);
436 double x_2 = std::cos(phi_2)*(l_motors*
cos(theta_2) + R);
437 double y_2 = -std::tan(phi_2)*x_2;
438 double z_2 = -l_motors*sin(theta_2);
441 double x_3 = std::cos(phi_3)*(l_motors*
cos(theta_3) + R);
442 double y_3 = -std::tan(phi_3)*x_3;
443 double z_3 = -l_motors*sin(theta_3);
448 double w_1 = std::pow(x_1,2) + std::pow(z_1,2);
449 double w_2 = std::pow(x_2,2) + std::pow(y_2,2) + std::pow(z_2,2);
450 double w_3 = std::pow(x_3,2) + std::pow(y_3,2) + std::pow(z_3,2);
452 double d = (x_1-x_2)*y_3 - (x_1-x_3)*y_2;
457 double a_1 = y_2*(z_1-z_3) - y_3*(z_1-z_2);
458 double b_1 = (y_3*(w_1-w_2) - y_2*(w_1-w_3))/2;
459 double a_2 = -(x_1-x_3)*(z_1-z_2)+(x_1-x_2)*(z_1-z_3);
460 double b_2 = ((x_1-x_3)*(w_1-w_2)-(x_1-x_2)*(w_1-w_3))/2;
463 double a = std::pow(a_1,2) + std::pow(a_2,2) + std::pow(d,2);
464 double b = 2*(a_1*b_1 + a_2*b_2 - z_1*std::pow(d,2) - x_1*a_1*d);
465 double c = std::pow(b_1,2) + std::pow(b_2,2) + std::pow(x_1,2)*std::pow(d,2) + std::pow(z_1,2)*std::pow(d,2) - 2*x_1*b_1*d - std::pow(l_ee,2)*std::pow(d,2);
466 double delta = std::pow(b,2) - 4*a*c;
471 ee_pose.z = -0.5*(b-std::sqrt(delta))/a;
472 ee_pose.x = (a_1*ee_pose.z + b_1)/d;
473 ee_pose.y = -(a_2*ee_pose.z + b_2)/d;
540 std::string position_joint = device_name +
"_shaft_joint";
541 std::string stiffness_joint = device_name +
"_stiffness_preset_virtual_joint";
544 motor_joint_names_.insert(std::make_pair(device_name, std::vector<std::string>({position_joint, stiffness_joint})));
546 std::string device_action = device_name +
"_position_and_preset_trajectory_controller/follow_joint_trajectory";
551 if (use_interactive_markers_) {
572 interactive_commands_server_ = std::make_unique<interactive_markers::InteractiveMarkerServer>(
"qbdelta_end_effector_interactive_commands");
574 controls_.header.frame_id =
"delta_base_frame_link";
575 controls_.name =
"qbdelta_end_effector_position_reference_controls";
576 controls_.description =
"qbdelta end-effector 3D pose reference.";
586 if (feedback->event_type == feedback->MOUSE_UP) {
590 if (feedback->event_type == feedback->MOUSE_DOWN) {
596 double feedback_distance = 0;
601 if (feedback_distance > 0.001) {
602 geometry_msgs::PointStamped target_pose;
603 target_pose.header = feedback->header;
604 target_pose.point = feedback->pose.position;
610 double R_motors = 0.1;
611 double R_ee = 0.0375;
612 double l_motors = 0.09;
615 double phi_2 = 2*M_PI/3;
616 double phi_3 = 4*M_PI/3;
617 double R = R_motors - R_ee;
618 joint_positions.resize(3);
620 auto arm_ik = [&](
const geometry_msgs::Point &ee_pose,
const double &phi,
double &joint_position) {
621 auto acos_safe = [](
const double &num,
const double &den,
double &
angle) {
622 angle = std::acos(num/den);
623 return std::abs(num) < std::abs(den);
626 double x = std::cos(phi)*ee_pose.x + std::sin(phi)*ee_pose.y - R;
627 double y = -std::sin(phi)*ee_pose.x + std::cos(phi)*ee_pose.y;
628 double z = ee_pose.z;
629 double theta_1, theta_2;
630 if (!acos_safe(y, l_ee, theta_1) || !acos_safe((std::pow(x,2) + std::pow(y,2) + std::pow(z,2) - std::pow(l_motors,2) - std::pow(l_ee,2)), (2*l_motors*l_ee*std::sin(theta_1)), theta_2)) {
633 double c_1 = l_ee*std::cos(theta_2)*std::sin(theta_1) + l_motors;
634 double c_2 = l_ee*std::sin(theta_2)*std::sin(theta_1);
635 joint_position = -(std::atan2(-c_2*x + c_1*z, c_1*x + c_2*z) - 1);
639 return arm_ik(ee_pose, phi_1, joint_positions.at(0)) && arm_ik(ee_pose, phi_2, joint_positions.at(1)) && arm_ik(ee_pose, phi_3, joint_positions.at(2));
644 move(trajectory.second, trajectory.first);
649 control_msgs::FollowJointTrajectoryGoal goal;
650 goal.trajectory = joint_trajectory;
658 if (xml_value.
size() != size) {
659 ROS_ERROR_STREAM_NAMED(
"delta_controller",
"Fails while setting the joint trajectory (joints size mismatch).");
662 for (
int j=0; j<xml_value.
size(); j++) {
663 vector.push_back(xmlCast<double>(xml_value[j]));
677 trajectory.second.points.erase(trajectory.second.points.begin(), trajectory.second.points.end()-1);
678 trajectory.second.points.front().time_from_start =
ros::Duration(0.1);
681 trajectory_msgs::JointTrajectory gripper_joint_trajectory;
682 for (
int i=0; i<waypoints.
size(); i++) {
683 trajectory_msgs::JointTrajectoryPoint point;
685 if (!waypoints[i].hasMember(
"time") && !waypoints[i].
hasMember(
"duration")) {
689 if (!waypoints[i].hasMember(
"gripper") || !
parseVector(waypoints[i][
"gripper"], 2, point.positions)) {
692 point.velocities = std::vector<double>(point.positions.size(), 0.0);
693 point.accelerations = std::vector<double>(point.positions.size(), 0.0);
696 for (
int j=0; j<(waypoints[i].
hasMember(
"time") ? waypoints[i][
"time"].
size() : waypoints[i][
"duration"].
size()); j++) {
697 point.time_from_start =
ros::Duration(waypoints[i].hasMember(
"time") ? static_cast<double>(waypoints[i][
"time"][j]) : (gripper_joint_trajectory.points.empty() ? 0 : gripper_joint_trajectory.points.back().time_from_start.toSec()) + static_cast<double>(waypoints[i][
"duration"][j]));
698 gripper_joint_trajectory.points.push_back(point);
701 if (!gripper_joint_trajectory.points.empty()) {
702 gripper_joint_trajectory.header.frame_id =
getMotorName(4);
703 gripper_joint_trajectory.header.stamp =
ros::Time(0);
705 std::map<std::string, trajectory_msgs::JointTrajectory> gripper_joint_trajectory_map;
706 gripper_joint_trajectory_map.insert(std::make_pair(
getMotorName(4), gripper_joint_trajectory));
710 for (
int i=0; i<waypoints.
size(); i++) {
711 if (!waypoints[i].hasMember(
"time") && !waypoints[i].
hasMember(
"duration")) {
715 std::vector<std::vector<double>> joint_positions;
716 std::vector<double> end_effector_position;
717 if (waypoints[i].hasMember(
"end_effector") &&
parseVector(waypoints[i][
"end_effector"], 3, end_effector_position)) {
718 geometry_msgs::Point target_pose;
719 target_pose.x = end_effector_position.at(0);
720 target_pose.y = end_effector_position.at(1);
721 target_pose.z = end_effector_position.at(2);
723 ROS_WARN_STREAM_NAMED(
"delta_controller",
"The Cartesian Linear Planner has produced a solution which cannot be inverted.");
731 std::vector<double> target_joint_stiffnesses;
732 if (!waypoints[i].hasMember(
"joint_stiffness") || !
parseVector(waypoints[i][
"joint_stiffness"], 3, target_joint_stiffnesses)) {
736 std::vector<std::vector<double>> joint_stiffnesses;
739 std::map<std::string, trajectory_msgs::JointTrajectory> motor_joint_trajectories;
745 for (
int j=1; j<(waypoints[i].
hasMember(
"time") ? waypoints[i][
"time"].
size() : waypoints[i][
"duration"].
size()); j++) {
763 std::vector<std::vector<double>> motor_positions;
776 geometry_msgs::Pose ee_pose;
790 return static_cast<bool>(xml_value);
793 return static_cast<double>(xml_value);
796 return static_cast<int>(xml_value);
geometry_msgs::Point ee_offset_
std::vector< geometry_msgs::Point > computeIntermediatePosesTo(const geometry_msgs::Point &target_pose)
Compute a linear-in-space interpolation from the last computed value to the given target end-effector...
void update(const ros::Time &time, const ros::Duration &period) override
Update the delta state.
std::map< std::string, std::vector< std::string > > motor_joint_names_
#define ROS_DEBUG_STREAM_NAMED(name, args)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_ERROR_STREAM_NAMED(name, args)
void initMarkers()
Initialize the interactive_markers::InteractiveMarkerServer and the marker displayed in rviz to contr...
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::AsyncSpinner spinner_
double computeDistance(const geometry_msgs::Point &from_pose, const geometry_msgs::Point &to_pose)
Compute the absolute 3D distance between the given points.
std::vector< double > getMotorStiffnesses()
void buildCube(visualization_msgs::InteractiveMarker &interactive_marker)
Create a cubic marker which can be moved interactively in all the Cartesian space.
void actionDoneCallback(const actionlib::SimpleClientGoalState &state, const control_msgs::FollowJointTrajectoryResultConstPtr &result, const std::string &controller)
Restart the waypoint trajectory automatically if waypoints have been retrieved during the initializat...
double getTrajectoryLastTimeFromStart()
#define ROS_INFO_STREAM_NAMED(name, args)
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
Type const & getType() const
std::unique_ptr< interactive_markers::InteractiveMarkerServer > interactive_commands_server_
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool cartesianLinearPlanner(const geometry_msgs::Point &target_pose, std::vector< std::vector< double >> &joint_positions)
Compute the joint trajectory of all the upper motors from the last computed value to the given target...
std::vector< std::string > getMotorJointNames(const int &id)
bool deltaStatePublisher(const std::vector< double > &motor_joints, const geometry_msgs::Point &ee_pose)
Compute all the free joint positions (i.e.
void filterMotorJointTrajectories(const std::vector< double > &b, const std::vector< double > &a)
Filter all the trajectories stored in the joint trajectories private map.
void interactiveMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
If waypoints are not used, this become the core method of the class, where commands are computed w...
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
double getMotorPosition(const int &id)
DeltaKinematicController()
Start the async spinner and do nothing else.
std::string toString() const
ros::NodeHandle node_handle_
ros::WallTimer waypoint_setup_timer_
bool init(hardware_interface::PositionJointInterface *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override
The init function is called to initialize the controller from a non-realtime thread with a pointer to...
void buildEndEffectorControl(visualization_msgs::InteractiveMarker &interactive_marker)
Add six distinct interactive controls to the given interactive marker.
std::map< std::string, trajectory_msgs::JointTrajectory > computeJointTrajectories(const std::vector< std::vector< double >> &motor_positions, const std::vector< std::vector< double >> &motor_stiffness, const double &target_time_from_start, const double &previous_time_from_start)
Compute a segment of trajectories for all the upper motors from the given parameters.
void filterMotorJointTrajectory(const std::vector< double > &b, const std::vector< double > &a, trajectory_msgs::JointTrajectory &motor_joint_trajectory)
Filter the given joint trajectory, by considering the whole data as a vector of new measurements (eac...
bool use_interactive_markers_
geometry_msgs::Point feedback_position_old_
ROSCPP_DECL bool get(const std::string &key, std::string &s)
ros::Subscriber target_poses_sub_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void setCallbackQueue(CallbackQueueInterface *queue)
T xmlCast(XmlRpc::XmlRpcValue xml_value)
Cast an XmlRpcValue from TypeDouble, TypeInt or TypeBoolean to the specified template type...
const std::string & getNamespace() const
std::string waypoint_namespace_
void targetPosesCallback(const geometry_msgs::PointStamped &msg)
Call the Cartesian Planner to compute the joint trajectory from the current position of the end-effec...
std::vector< double > getMotorPositions()
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::vector< std::vector< double > > computeIntermediateStiffnessesTo(const std::vector< double > &target_joint_stiffnesses, const int &size)
Compute the linearized trajectory from the last computed value to the given target joint stiffness of...
std::map< std::string, trajectory_msgs::JointTrajectory > motor_joint_trajectories_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle node_handle_control_
std::vector< std::string > motor_names_
TFSIMD_FORCE_INLINE const tfScalar & z() const
void startWaypointTrajectory()
Parse all the waypoints, filter the whole generated joint trajectory and send the commands to the dev...
void actionFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &feedback, const std::string &controller)
Do nothing apart from debug info.
double getMotorStiffness(const int &id)
JointHandle getHandle(const std::string &name)
bool hasMember(const std::string &name) const
~DeltaKinematicController() override
Stop the async spinner.
bool parseVector(const XmlRpc::XmlRpcValue &xml_value, const int &size, std::vector< double > &vector)
Parse the given XmlRpcValue as a std::vector, since the XmlRpc::XmlRpcValue class does not handle thi...
std::string getMotorName(const int &id)
std::map< std::string, hardware_interface::JointHandle > motor_joints_
bool inverseKinematics(const geometry_msgs::Point &ee_pose, std::vector< double > &joint_positions)
Compute the inverse kinematics of the delta device.
std::vector< double > filter_param_a_
void fillMotorJointTrajectories(const std::map< std::string, trajectory_msgs::JointTrajectory > &motor_joint_trajectories)
Append the given trajectories to the joint trajectories private map.
void move()
Make all the calls to the Action Servers relative to all the trajectories previously parsed (either f...
bool getParam(const std::string &key, std::string &s) const
std::vector< double > getTrajectoryLastStiffnesses()
This controller is made to simplify the usage and the user interaction with the delta device...
void actionActiveCallback(const std::string &controller)
Do nothing apart from debug info.
void parseWaypointTrajectory(ros::NodeHandle &node_handle)
Parse all the waypoints set up in the Parameter Server at <robot_namespace>/waypoints.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
std::vector< double > getTrajectoryLastPositions()
visualization_msgs::InteractiveMarker controls_
std::vector< double > filter_param_b_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void filter(const std::vector< double > &b, const std::vector< double > &a, const std::vector< double > &x, std::vector< double > &y)
Filter data following a discrete parametrization of the samples.
bool forwardKinematics(const std::vector< double > &joint_positions, geometry_msgs::Point &ee_pose)
Compute the forward kinematics of the delta device.
ros::CallbackQueuePtr callback_queue_
ros::Publisher free_joint_state_publisher_
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
tf2_ros::TransformBroadcaster tf_broadcaster_
trajectory_msgs::JointTrajectory getMotorJointTrajectory(const int &id)
std::map< std::string, std::unique_ptr< actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > > > motor_action_clients_
#define ROS_WARN_STREAM_NAMED(name, args)