This controller is made to simplify the usage and the user interaction with the delta device. More...
#include <qb_delta_kinematic_controller.h>
Public Member Functions | |
DeltaKinematicController () | |
Start the async spinner and do nothing else. More... | |
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 the hardware interface itself, instead of a pointer to a RobotHW. More... | |
void | starting (const ros::Time &time) override |
This is called from within the realtime thread just before the first call to update. More... | |
void | stopping (const ros::Time &time) override |
This is called from within the realtime thread just after the last update call before the controller is stopped. More... | |
void | update (const ros::Time &time, const ros::Duration &period) override |
Update the delta state. More... | |
~DeltaKinematicController () override | |
Stop the async spinner. More... | |
Public Member Functions inherited from controller_interface::Controller< hardware_interface::PositionJointInterface > | |
Controller () | |
virtual bool | init (hardware_interface::PositionJointInterface *, ros::NodeHandle &) |
virtual | ~Controller () |
Public Member Functions inherited from controller_interface::ControllerBase | |
ControllerBase () | |
bool | isRunning () |
bool | isRunning () |
bool | startRequest (const ros::Time &time) |
bool | startRequest (const ros::Time &time) |
bool | stopRequest (const ros::Time &time) |
bool | stopRequest (const ros::Time &time) |
void | updateRequest (const ros::Time &time, const ros::Duration &period) |
void | updateRequest (const ros::Time &time, const ros::Duration &period) |
virtual | ~ControllerBase () |
Protected Member Functions | |
void | actionActiveCallback (const std::string &controller) |
Do nothing apart from debug info. More... | |
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 initialization. More... | |
void | actionFeedbackCallback (const control_msgs::FollowJointTrajectoryFeedbackConstPtr &feedback, const std::string &controller) |
Do nothing apart from debug info. More... | |
void | buildCube (visualization_msgs::InteractiveMarker &interactive_marker) |
Create a cubic marker which can be moved interactively in all the Cartesian space. More... | |
void | buildEndEffectorControl (visualization_msgs::InteractiveMarker &interactive_marker) |
Add six distinct interactive controls to the given interactive marker. More... | |
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 end-effector position in a linearized fashion w.r.t. More... | |
double | computeDistance (const geometry_msgs::Point &from_pose, const geometry_msgs::Point &to_pose) |
Compute the absolute 3D distance between the given points. More... | |
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 position. More... | |
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 all the upper motors. More... | |
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. More... | |
bool | deltaStatePublisher (const std::vector< double > &motor_joints, const geometry_msgs::Point &ee_pose) |
Compute all the free joint positions (i.e. More... | |
void | fillMotorJointTrajectories (const std::map< std::string, trajectory_msgs::JointTrajectory > &motor_joint_trajectories) |
Append the given trajectories to the joint trajectories private map. More... | |
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. More... | |
void | filterMotorJointTrajectories (const std::vector< double > &b, const std::vector< double > &a) |
Filter all the trajectories stored in the joint trajectories private map. More... | |
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 (each point is a sample). More... | |
bool | forwardKinematics (const std::vector< double > &joint_positions, geometry_msgs::Point &ee_pose) |
Compute the forward kinematics of the delta device. More... | |
std::vector< std::string > | getMotorJointNames (const int &id) |
trajectory_msgs::JointTrajectory | getMotorJointTrajectory (const int &id) |
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< double > | getTrajectoryLastPositions () |
std::vector< double > | getTrajectoryLastStiffnesses () |
double | getTrajectoryLastTimeFromStart () |
void | initMarkers () |
Initialize the interactive_markers::InteractiveMarkerServer and the marker displayed in rviz to control the end-effector position of the delta device. More... | |
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.r.t. More... | |
bool | inverseKinematics (const geometry_msgs::Point &ee_pose, std::vector< double > &joint_positions) |
Compute the inverse kinematics of the delta device. More... | |
void | move (const trajectory_msgs::JointTrajectory &joint_trajectory, const std::string &motor) |
Build a control_msgs::FollowJointTrajectoryGoal with the given joint trajectory and make a call to the Action Server relative to the provided motor controller (using the just created goal). More... | |
void | move () |
Make all the calls to the Action Servers relative to all the trajectories previously parsed (either from waypoint or interactive markers). More... | |
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 this conversion yet. More... | |
void | parseWaypointTrajectory (ros::NodeHandle &node_handle) |
Parse all the waypoints set up in the Parameter Server at <robot_namespace>/waypoints. More... | |
void | startWaypointTrajectory () |
Parse all the waypoints, filter the whole generated joint trajectory and send the commands to the devices through their relative Action server. More... | |
void | targetPosesCallback (const geometry_msgs::PointStamped &msg) |
Call the Cartesian Planner to compute the joint trajectory from the current position of the end-effector to the given target. More... | |
template<class T > | |
T | xmlCast (XmlRpc::XmlRpcValue xml_value) |
Cast an XmlRpcValue from TypeDouble , TypeInt or TypeBoolean to the specified template type. More... | |
Protected Member Functions inherited from controller_interface::Controller< hardware_interface::PositionJointInterface > | |
std::string | getHardwareInterfaceType () const |
virtual bool | initRequest (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources) |
Additional Inherited Members | |
Public Types inherited from controller_interface::ControllerBase | |
typedef std::vector< hardware_interface::InterfaceResources > | ClaimedResources |
Public Attributes inherited from controller_interface::ControllerBase | |
CONSTRUCTED | |
INITIALIZED | |
RUNNING | |
enum controller_interface::ControllerBase:: { ... } | state_ |
This controller is made to simplify the usage and the user interaction with the delta device.
Actually it is an upper level controller which relies on the low-level trajectory controller provided by the qbMoveHW
interfaces. This controller also compute the forward kinematics to properly visualize the whole kinematic structure in rviz
. Additionally it provides waypoints control mode (similar to the single device and chained mode), and an interactive marker control mode (similar to the one of the single qbmove device).
Definition at line 58 of file qb_delta_kinematic_controller.h.
DeltaKinematicController::DeltaKinematicController | ( | ) |
Start the async spinner and do nothing else.
The real initialization is done later by init()
.
Definition at line 32 of file qb_delta_kinematic_controller.cpp.
|
override |
Stop the async spinner.
Definition at line 40 of file qb_delta_kinematic_controller.cpp.
|
protected |
Do nothing apart from debug info.
controller | The action controller name. |
Definition at line 44 of file qb_delta_kinematic_controller.cpp.
|
protected |
Restart the waypoint trajectory automatically if waypoints have been retrieved during the initialization.
state | The final state of the action. |
result | The error code and error message (0 if succeeds). |
controller | The action controller name. |
Definition at line 48 of file qb_delta_kinematic_controller.cpp.
|
protected |
Do nothing apart from debug info.
feedback | The action feedback state. |
controller | The action controller name. |
Definition at line 73 of file qb_delta_kinematic_controller.cpp.
|
protected |
Create a cubic marker which can be moved interactively in all the Cartesian space.
The marker represent the position of the end-effector of the delta.
interactive_marker | The Interactive Marker structure to be filled. |
Definition at line 79 of file qb_delta_kinematic_controller.cpp.
|
protected |
Add six distinct interactive controls to the given interactive marker.
They let to move the marker in the Cartesian space by limiting the motion to a single principal axis or plane, e.g. only along x axis or in the yz plane.
interactive_marker | The Interactive Marker structure to be filled. |
Definition at line 98 of file qb_delta_kinematic_controller.cpp.
|
protected |
Compute the joint trajectory of all the upper motors from the last computed value to the given target end-effector position in a linearized fashion w.r.t.
to the Cartesian space. If there are no last values, current shaft positions of the upper motors are taken.
target_pose | The target end-effector position. | |
[out] | joint_positions | The vector of all the shaft position of all the upper motors. |
true
on success. Definition at line 132 of file qb_delta_kinematic_controller.cpp.
|
protected |
Compute the absolute 3D distance between the given points.
from_pose | The point from which to compute the distance. |
to_pose | The point to which to compute the distance. |
Definition at line 154 of file qb_delta_kinematic_controller.cpp.
|
protected |
Compute a linear-in-space interpolation from the last computed value to the given target end-effector position.
If there are no last values stored, current joint positions are taken to compute the forward kinematics.
target_pose | The target end-effector position. |
Definition at line 158 of file qb_delta_kinematic_controller.cpp.
|
protected |
Compute the linearized trajectory from the last computed value to the given target joint stiffness of all the upper motors.
If there are no last values stored, current stiffness values are taken.
target_joint_stiffnesses | The target stiffness value of all the upper motors. |
size | The number of points that needs to be generated. |
Definition at line 180 of file qb_delta_kinematic_controller.cpp.
|
protected |
Compute a segment of trajectories for all the upper motors from the given parameters.
motor_positions
must be strictly related to motor_stiffness
, i.e. same size and meanings. motor_positions | The vector of shaft positions of all the upper motors. |
motor_stiffness | The vector of stiffness values of all the upper motors. |
target_time_from_start | The final time_from_start of this segment of trajectory. |
previous_time_from_start | The initial time_from_start of this segment of trajectory. |
Definition at line 200 of file qb_delta_kinematic_controller.cpp.
|
protected |
Compute all the free joint positions (i.e.
of the arms) of the delta device and publish them to the Joint State Publisher. Also update the end-effector floating tf
frame with the given end-effector position.
motor_joints | The upper motor position. |
ee_pose | The end-effector position w.r.t. the upper plate. |
true
on success. Definition at line 222 of file qb_delta_kinematic_controller.cpp.
|
protected |
Append the given trajectories to the joint trajectories private map.
motor_joint_trajectories | The map with new joint trajectories (either for the gripper and upper motors). |
Definition at line 282 of file qb_delta_kinematic_controller.cpp.
|
protected |
Filter data following a discrete parametrization of the samples.
b | The vector of numerator coefficients, i.e. the ones applied to the new measurements. | |
a | The vector of denominator coefficients, i.e. the ones applied to the old data. | |
x | The new measurements. | |
[out] | y | The whole filtered data. |
Definition at line 292 of file qb_delta_kinematic_controller.cpp.
|
protected |
Filter all the trajectories stored in the joint trajectories private map.
b | The vector of numerator coefficients, i.e. the ones applied to the new measurements. |
a | The vector of denominator coefficients, i.e. the ones applied to the old data. |
Definition at line 357 of file qb_delta_kinematic_controller.cpp.
|
protected |
Filter the given joint trajectory, by considering the whole data as a vector of new measurements (each point is a sample).
b | The vector of numerator coefficients, i.e. the ones applied to the new measurements. | |
a | The vector of denominator coefficients, i.e. the ones applied to the old data. | |
[out] | motor_joint_trajectory | The joint trajectory to be filtered which is modified in place. |
Definition at line 366 of file qb_delta_kinematic_controller.cpp.
|
protected |
Compute the forward kinematics of the delta device.
joint_positions | The upper motor position. | |
[out] | ee_pose | The computed end-effector position w.r.t. the upper plate. |
true
on success. Definition at line 404 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 497 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 501 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 477 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 481 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 485 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 489 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 493 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 505 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 512 of file qb_delta_kinematic_controller.cpp.
|
protected |
time_from_start
(0
if empty). Definition at line 519 of file qb_delta_kinematic_controller.cpp.
|
overridevirtual |
The init function is called to initialize the controller from a non-realtime thread with a pointer to the hardware interface itself, instead of a pointer to a RobotHW.
robot_hw | The specific hardware interface used by this controller. |
root_nh | A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services). |
controller_nh | A NodeHandle in the namespace from which the controller should read its configuration, and where it should set up its ROS interface. |
true
if initialization was successful and the controller is ready to be started. Reimplemented from controller_interface::Controller< hardware_interface::PositionJointInterface >.
Definition at line 526 of file qb_delta_kinematic_controller.cpp.
|
protected |
Initialize the interactive_markers::InteractiveMarkerServer
and the marker displayed in rviz
to control the end-effector position of the delta device.
This method is called during the controller initialization if required by the user (cannot be started whether waypoints are active).
Definition at line 571 of file qb_delta_kinematic_controller.cpp.
|
protected |
If waypoints are not used, this become the core method of the class, where commands are computed w.r.t.
the previous state of the interactive marker and the computed joint trajectory is sent to each low level controllers, e.g. the joint trajectory controller of each qbMoveHW
.
feedback | The feedback state of the interactive marker, provided by rviz. |
Definition at line 585 of file qb_delta_kinematic_controller.cpp.
|
protected |
Compute the inverse kinematics of the delta device.
ee_pose | The end-effector position w.r.t. the upper plate. | |
[out] | joint_positions | The computed upper motor position. |
true
on success. Definition at line 609 of file qb_delta_kinematic_controller.cpp.
|
protected |
Build a control_msgs::FollowJointTrajectoryGoal
with the given joint trajectory and make a call to the Action Server relative to the provided motor controller (using the just created goal).
joint_trajectory | The waypoint trajectory properly filled for all the joints of the controller. |
motor | The action controller name. |
Definition at line 648 of file qb_delta_kinematic_controller.cpp.
|
protected |
Make all the calls to the Action Servers relative to all the trajectories previously parsed (either from waypoint or interactive markers).
Definition at line 642 of file qb_delta_kinematic_controller.cpp.
|
protected |
Parse the given XmlRpcValue
as a std::vector
, since the XmlRpc::XmlRpcValue
class does not handle this conversion yet.
xml_value | The value retrieved from the Parameter Server to be parsed as a std::vector . | |
controller | The action controller name. | |
[out] | vector | The std::vector parsed from the XmlRpcValue . |
true
on success. Definition at line 657 of file qb_delta_kinematic_controller.cpp.
|
protected |
Parse all the waypoints set up in the Parameter Server at <robot_namespace>/waypoints.
The waypoints are composed by a vector where each element must contains:
time
which can be either a single value or an interval for which other values hold; And one or more of the followings:end_effector
containing [x, y, z] coordinates for the delta end-effector;joint_stiffness
containing the stiffness value for each of the three upper motors;gripper
filled with the gripper reference position and its stiffness value; Whenever these are not present in the Parameter Server, it is assigned the last available value. node_handle | The Node Handle in which the waypoints are parsed, under .../waypoints (if present). |
Definition at line 668 of file qb_delta_kinematic_controller.cpp.
|
inlineoverridevirtual |
This is called from within the realtime thread just before the first call to update.
time | The current time. |
Reimplemented from controller_interface::ControllerBase.
Definition at line 87 of file qb_delta_kinematic_controller.h.
|
protected |
Parse all the waypoints, filter the whole generated joint trajectory and send the commands to the devices through their relative Action server.
This can be helpful in a cyclic waypoint behavior to recompute the trajectory from the last reached position (which can be different from the initial one).
Definition at line 753 of file qb_delta_kinematic_controller.cpp.
|
inlineoverridevirtual |
This is called from within the realtime thread just after the last update call before the controller is stopped.
time | The current time. |
Reimplemented from controller_interface::ControllerBase.
Definition at line 93 of file qb_delta_kinematic_controller.h.
|
protected |
Call the Cartesian Planner to compute the joint trajectory from the current position of the end-effector to the given target.
The trajectory is filled in a proper structure which is sent to the Action server of each qbmove.
msg | The target end-effector position. |
Definition at line 762 of file qb_delta_kinematic_controller.cpp.
|
overridevirtual |
Update the delta state.
This is called periodically by the realtime thread when the controller is running.
time | The current time. |
period | The time passed since the last call to update. |
Implements controller_interface::ControllerBase.
Definition at line 773 of file qb_delta_kinematic_controller.cpp.
|
protected |
Cast an XmlRpcValue
from TypeDouble
, TypeInt
or TypeBoolean
to the specified template type.
This is necessary since XmlRpc does not handle conversion among basic types: it throws an exception if an improper cast is invoked though (e.g. from int to double).
T | The type to cast to. |
xml_value | The wrong-casted value. |
Definition at line 787 of file qb_delta_kinematic_controller.cpp.
|
protected |
Definition at line 103 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 113 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 122 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 115 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 114 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 129 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 130 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 128 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 107 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 112 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 127 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 118 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 120 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 121 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 119 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 117 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 105 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 106 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 104 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 108 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 110 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 124 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 125 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 126 of file qb_delta_kinematic_controller.h.
|
protected |
Definition at line 109 of file qb_delta_kinematic_controller.h.