28 #ifndef QB_CHAIN_CONTROLLERS_H 29 #define QB_CHAIN_CONTROLLERS_H 36 #include <control_msgs/FollowJointTrajectoryAction.h> 38 #include <geometry_msgs/PointStamped.h> 42 #include <sensor_msgs/JointState.h> 46 #include <trajectory_msgs/JointTrajectory.h> 118 std::map<std::string, std::unique_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>>>
motor_action_clients_;
152 void actionFeedbackCallback(
const control_msgs::FollowJointTrajectoryFeedbackConstPtr &feedback,
const std::string &controller);
160 void buildCube(visualization_msgs::InteractiveMarker &interactive_marker);
179 bool cartesianLinearPlanner(
const geometry_msgs::Point &target_pose, std::vector<std::vector<double>> &joint_positions);
187 double computeDistance(
const geometry_msgs::Point &from_pose,
const geometry_msgs::Point &to_pose);
217 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);
227 bool deltaStatePublisher(
const std::vector<double> &motor_joints,
const geometry_msgs::Point &ee_pose);
243 void filter(
const std::vector<double> &b,
const std::vector<double> &a,
const std::vector<double> &x, std::vector<double> &y);
261 void filterMotorJointTrajectory(
const std::vector<double> &b,
const std::vector<double> &a, trajectory_msgs::JointTrajectory &motor_joint_trajectory);
270 bool forwardKinematics(
const std::vector<double> &joint_positions, geometry_msgs::Point &ee_pose);
346 bool inverseKinematics(
const geometry_msgs::Point &ee_pose, std::vector<double> &joint_positions);
355 void move(
const trajectory_msgs::JointTrajectory &joint_trajectory,
const std::string &motor);
422 #endif // QB_CHAIN_CONTROLLERS_H 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_
void initMarkers()
Initialize the interactive_markers::InteractiveMarkerServer and the marker displayed in rviz to contr...
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()
std::unique_ptr< interactive_markers::InteractiveMarkerServer > interactive_commands_server_
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...
double getMotorPosition(const int &id)
DeltaKinematicController()
Start the async spinner and do nothing else.
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_
ros::Subscriber target_poses_sub_
T xmlCast(XmlRpc::XmlRpcValue xml_value)
Cast an XmlRpcValue from TypeDouble, TypeInt or TypeBoolean to the specified template type...
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()
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_
ros::NodeHandle node_handle_control_
std::vector< std::string > motor_names_
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)
~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...
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.
void starting(const ros::Time &time) override
This is called from within the realtime thread just before the first call to update.
std::vector< double > getTrajectoryLastPositions()
visualization_msgs::InteractiveMarker controls_
std::vector< double > filter_param_b_
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_
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_
void stopping(const ros::Time &time) override
This is called from within the realtime thread just after the last update call before the controller ...