#include <joint_position_integrator.hpp>
Public Member Functions | |
JointPositionIntegrator (ros::NodeHandle &nh) | |
void | reset () |
KDL::JntArray | update (const KDL::JntArray &q_dot, const KDL::JntArray &q, float dt) |
Eigen::VectorXd | update (const Eigen::VectorXd &q_dot, const Eigen::VectorXd &q, float dt) |
~JointPositionIntegrator () | |
Private Member Functions | |
bool | init () |
Private Attributes | |
bool | got_state_ |
float | max_joint_error_ |
ros::NodeHandle | nh_ |
KDL::JntArray | virt_q_ |
Implement a joint position integrator which produces joint position references given a joint velocity target.
Definition at line 13 of file joint_position_integrator.hpp.
generic_control_toolbox::JointPositionIntegrator::JointPositionIntegrator | ( | ros::NodeHandle & | nh | ) |
Definition at line 5 of file joint_position_integrator.cpp.
|
inline |
Definition at line 17 of file joint_position_integrator.hpp.
|
private |
Get the parameters for the integrator.
Definition at line 67 of file joint_position_integrator.cpp.
void generic_control_toolbox::JointPositionIntegrator::reset | ( | ) |
Reset the integrator's state.
Definition at line 16 of file joint_position_integrator.cpp.
KDL::JntArray generic_control_toolbox::JointPositionIntegrator::update | ( | const KDL::JntArray & | q_dot, |
const KDL::JntArray & | q, | ||
float | dt | ||
) |
Produce an updated joint position vector for a given set of joint velocities.
The output is q = sat(q + q_dot * dt), where the function sat(.) saturates the output if any joint exceeds the configured max_joint_error.
q_dot | The array of joint velocities to be integrated. |
q | The current joint state. |
dt | The integration delta. |
Definition at line 18 of file joint_position_integrator.cpp.
Eigen::VectorXd generic_control_toolbox::JointPositionIntegrator::update | ( | const Eigen::VectorXd & | q_dot, |
const Eigen::VectorXd & | q, | ||
float | dt | ||
) |
Definition at line 43 of file joint_position_integrator.cpp.
|
private |
Definition at line 45 of file joint_position_integrator.hpp.
|
private |
Definition at line 44 of file joint_position_integrator.hpp.
|
private |
Definition at line 42 of file joint_position_integrator.hpp.
|
private |
Definition at line 43 of file joint_position_integrator.hpp.