qb_delta_kinematic_controller.h

Go to the documentation of this file.

/***
 *  Software License Agreement: BSD 3-Clause License
 *  
 *  Copyright (c) 2016-2018, qbrobotics®
 *  All rights reserved.
 *  
 *  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 *  following conditions are met:
 *  
 *  * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *    following disclaimer.
 *  
 *  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *    following disclaimer in the documentation and/or other materials provided with the distribution.
 *  
 *  * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote
 *    products derived from this software without specific prior written permission.
 *  
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 *  USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef QB_CHAIN_CONTROLLERS_H
#define QB_CHAIN_CONTROLLERS_H

// Standard libraries
#include <regex>

// ROS libraries
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/PointStamped.h>
#include <hardware_interface/joint_command_interface.h>
#include <interactive_markers/interactive_marker_server.h>
#include <pluginlib/class_list_macros.hpp>
#include <sensor_msgs/JointState.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <trajectory_msgs/JointTrajectory.h>

// internal libraries

namespace qb_chain_controllers {
class DeltaKinematicController : public controller_interface::Controller<hardware_interface::PositionJointInterface> {
 public:
  DeltaKinematicController();

  ~DeltaKinematicController() override;

  bool init(hardware_interface::PositionJointInterface *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override;

  void starting(const ros::Time &time) override {}

  void stopping(const ros::Time &time) override {}

  void update(const ros::Time &time, const ros::Duration &period) override;

 protected:
  ros::CallbackQueuePtr callback_queue_;
  ros::AsyncSpinner spinner_;
  ros::NodeHandle node_handle_;
  ros::NodeHandle node_handle_control_;
  ros::Publisher free_joint_state_publisher_;
  ros::Subscriber target_poses_sub_;
  ros::WallTimer waypoint_setup_timer_;
  tf2_ros::TransformBroadcaster tf_broadcaster_;

  std::unique_ptr<interactive_markers::InteractiveMarkerServer> interactive_commands_server_;
  visualization_msgs::InteractiveMarker controls_;
  geometry_msgs::Point feedback_position_old_;
  bool feedback_active_;

  std::vector<std::string> motor_names_;
  std::map<std::string, std::unique_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>>> motor_action_clients_;
  std::map<std::string, hardware_interface::JointHandle> motor_joints_;
  std::map<std::string, std::vector<std::string>> motor_joint_names_;
  std::map<std::string, trajectory_msgs::JointTrajectory> motor_joint_trajectories_;
  geometry_msgs::Point ee_offset_;

  bool use_interactive_markers_;
  bool use_waypoints_;
  std::string waypoint_namespace_;
  double max_displacement_;
  bool filter_trajectory_;
  std::vector<double> filter_param_a_;
  std::vector<double> filter_param_b_;

  void actionActiveCallback(const std::string &controller);

  void actionDoneCallback(const actionlib::SimpleClientGoalState &state, const control_msgs::FollowJointTrajectoryResultConstPtr &result, const std::string &controller);

  void actionFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &feedback, const std::string &controller);

  void buildCube(visualization_msgs::InteractiveMarker &interactive_marker);

  void buildEndEffectorControl(visualization_msgs::InteractiveMarker &interactive_marker);

  bool cartesianLinearPlanner(const geometry_msgs::Point &target_pose, std::vector<std::vector<double>> &joint_positions);

  double computeDistance(const geometry_msgs::Point &from_pose, const geometry_msgs::Point &to_pose);

  std::vector<geometry_msgs::Point> computeIntermediatePosesTo(const geometry_msgs::Point &target_pose);

  std::vector<std::vector<double>> computeIntermediateStiffnessesTo(const std::vector<double> &target_joint_stiffnesses, const int &size);

  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);

  bool deltaStatePublisher(const std::vector<double> &motor_joints, const geometry_msgs::Point &ee_pose);

  void fillMotorJointTrajectories(const std::map<std::string, trajectory_msgs::JointTrajectory> &motor_joint_trajectories);

  void filter(const std::vector<double> &b, const std::vector<double> &a, const std::vector<double> &x, std::vector<double> &y);

  void filterMotorJointTrajectories(const std::vector<double> &b, const std::vector<double> &a);

  void filterMotorJointTrajectory(const std::vector<double> &b, const std::vector<double> &a, trajectory_msgs::JointTrajectory &motor_joint_trajectory);

  bool forwardKinematics(const std::vector<double> &joint_positions, geometry_msgs::Point &ee_pose);

  std::string getMotorName(const int &id);

  double getMotorPosition(const int &id);

  std::vector<double> getMotorPositions();

  double getMotorStiffness(const int &id);

  std::vector<double> getMotorStiffnesses();

  std::vector<std::string> getMotorJointNames(const int &id);

  trajectory_msgs::JointTrajectory getMotorJointTrajectory(const int &id);

  std::vector<double> getTrajectoryLastPositions();

  std::vector<double> getTrajectoryLastStiffnesses();

  double getTrajectoryLastTimeFromStart();

  void initMarkers();

  void interactiveMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);

  bool inverseKinematics(const geometry_msgs::Point &ee_pose, std::vector<double> &joint_positions);

  void move(const trajectory_msgs::JointTrajectory &joint_trajectory, const std::string &motor);

  void move();

  bool parseVector(const XmlRpc::XmlRpcValue &xml_value, const int &size, std::vector<double> &vector);

  void parseWaypointTrajectory(ros::NodeHandle &node_handle);

  void startWaypointTrajectory();
  void targetPosesCallback(const geometry_msgs::PointStamped &msg);

  template<class T>
  T xmlCast(XmlRpc::XmlRpcValue xml_value);
};
}  // namespace qb_chain_controllers

#endif // QB_CHAIN_CONTROLLERS_H