[DoNotMerge] Twist controller extensions #121
Conversation
…ss).. we might change this one to maximize the manipulability rate to find a better value for the damping_gain which we introduced to the pseudo inverse calculations. The main change in the code is the additional constriant.
|
I will formally and syntactically work on this PR. @ipa-bfb could you afterwards explain the new features to me as you worked with @ipa-fxm-cm on this... |
| # ==================================== Controller interfaces ================================================================================= | ||
| ctrl_interface = gen.add_group("Controller Interfaces", "ctrl_interface") | ||
| ctrl_interface.add("controller_interface", int_t, 0, "The controller interface to use", 0, None, None, edit_method=controller_interface_enum) | ||
| ctrl_interface.add("integrator_smoothing", double_t, 0, "The factor used for exponential smoothing during simpson integration)", 0.2, 0, 1) | ||
|
|
||
| # ==================================== Damping and truncation (singular value adaption) ==================================================== | ||
| damp_trunc = gen.add_group("Damping and Truncation", "damping_truncation") | ||
| damp_trunc.add("numerical_filtering", bool_t, 0, "Numerical Filtering yes/no", False) | ||
| damp_trunc.add("damping_method", int_t, 0, "The damping method to use.", 2, None, None, edit_method=damping_method_enum) | ||
| damp_trunc.add("singularity_avoidance", int_t, 0, "Singularity avoidance method", 0, None, None, edit_method=singularity_avoidance_enum) |
There was a problem hiding this comment.
@ipa-bfb
Why has numerical_filtering been removed?
Why is singularity_avoidance now an option within Damping and Truncation?
| /* BEGIN JointSingularityAvoidance ************************************************************************************/ | ||
| /// Class providing methods that realize a JointSingularityAvoidance constraint. | ||
| template <typename T_PARAMS, typename PRIO = uint32_t> | ||
| class JointSingularityAvoidance : public ConstraintBase<T_PARAMS, PRIO> |
There was a problem hiding this comment.
@ipa-bfb
JointSingularityAvoidance is a ConstraintBase so no DampingBase!
To me this sounds like the JSA should be configurable as as Constraint and thus not mess up with the DampingMethods, i.e. in the cfg
| damping_method(MANIPULABILITY), | ||
| damping_factor(0.2), | ||
| lambda_max(0.1), | ||
| w_threshold(0.005), | ||
| beta(0.005), | ||
| eps_damping(0.003), | ||
| eps_truncation(0.001), | ||
| damping_delta(0.1), | ||
| damping_gain(0.02), | ||
| damping_slope(0.05), |
There was a problem hiding this comment.
@ipa-bfb
These parameters are used within JointSingularityAvoidance, right....shouldn't they then be named jsa_delta, jsa_gain, jsa_slope?
| DampingMethodTypes damping_method; | ||
| double damping_factor; | ||
| double lambda_max; | ||
| double w_threshold; | ||
| double beta; | ||
| double eps_damping; | ||
| double eps_truncation; | ||
| double damping_delta; | ||
| double damping_gain; | ||
| double damping_slope; |
There was a problem hiding this comment.
same as comment above
| @@ -278,6 +292,8 @@ struct TwistControllerParams | |||
|
|
|||
| // vector of links of the chain to be considered for collision avoidance | |||
| std::vector<std::string> collision_check_links; | |||
|
|
|||
| KDL::Chain chain; | |||
There was a problem hiding this comment.
@ipa-bfb
What is this used for? Does this need to be a class member?
There was a problem hiding this comment.
removed as it is not used anywhere
| KDL::ChainFkSolverVel_recursive& fk_solver_vel_; | ||
|
|
||
| }; | ||
| /* END JointLimitAvoidance **************************************************************************************/ |
| @@ -140,7 +140,8 @@ class ConstraintBase : public PriorityBase<PRIO> | |||
| prediction_value_(std::numeric_limits<double>::max()), | |||
| last_value_(0.0), | |||
| last_time_(ros::Time::now()), | |||
| last_pred_time_(ros::Time::now()) | |||
| last_pred_time_(ros::Time::now()), | |||
| init_(false) | |||
There was a problem hiding this comment.
these things are constraint-specific! So do not put them in the base class!
There was a problem hiding this comment.
removed because unused anyway
| @@ -222,6 +223,8 @@ class ConstraintBase : public PriorityBase<PRIO> | |||
| JointStates joint_states_; | |||
| KDL::JntArrayVel jnts_prediction_; | |||
| Matrix6Xd_t jacobian_data_; | |||
| Matrix6Xd_t jacobian_data_old_; | |||
| bool init_; | |||
There was a problem hiding this comment.
removed as both variables are nowhere used
| @@ -256,7 +259,7 @@ class ConstraintBase : public PriorityBase<PRIO> | |||
| adapted_params.damping_method = CONSTANT; | |||
| adapted_params.damping_factor = const_damping_factor; | |||
| adapted_params.eps_truncation = 0.0; | |||
| adapted_params.numerical_filtering = false; | |||
| // adapted_params.numerical_filtering = false; | |||
There was a problem hiding this comment.
I definitely want to keep numerical_filtering!
| @@ -287,7 +287,8 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues() | |||
| { | |||
| if (params.frame_names.end() != str_it) | |||
| { | |||
| Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector; | |||
| Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector; | |||
There was a problem hiding this comment.
@ipa-bfb
Why was this computation changed? Why don't we subtract it->frame_vector anymore?
| @@ -308,7 +309,8 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues() | |||
| uint32_t frame_number = idx + 1; // segment nr not index represents frame number | |||
|
|
|||
| KDL::JntArray ja = this->joint_states_.current_q_; | |||
| KDL::Jacobian new_jac_chain(this->joint_states_.current_q_.rows()); | |||
| ja.resize((unsigned int)params.dof); | |||
| KDL::Jacobian new_jac_chain(ja.rows()); | |||
There was a problem hiding this comment.
Is the dimension still correct when being used with a "KinematicExtension"?
| @@ -389,22 +391,21 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue() | |||
|
|
|||
| if (params.frame_names.end() != str_it) | |||
| { | |||
| unsigned int dof; | |||
There was a problem hiding this comment.
this variable is not used!
| if (this->constraint_params_.current_distances_.size() > 0) | ||
| { | ||
| uint32_t frame_number = (str_it - params.frame_names.begin()) + 1; // segment nr not index represents frame number | ||
| KDL::FrameVel frame_vel; | ||
|
|
||
| // ToDo: the fk_solver_vel_ is only initialized for the primary chain - kinematic extensions cannot be considered yet! |
There was a problem hiding this comment.
@ipa-bfb
Has this been fixed? Otherwise keep the comment!
There was a problem hiding this comment.
Is not fixed yet....keeping comment and debug output below
| // Calculate prediction for pos and vel | ||
| int error = this->fk_solver_vel_.JntToCart(this->jnts_prediction_, frame_vel, frame_number); | ||
| // Calculate prediction for the mainipulator | ||
| int error = this->fk_solver_vel_.JntToCart(jnts_prediction_chain, frame_vel, frame_number); |
There was a problem hiding this comment.
@ipa-bfb
Needs explanation
| } | ||
|
|
||
|
|
||
| KDL::Twist twist = frame_vel.GetTwist() + predicted_twist_odometry; // predicted frame twist |
There was a problem hiding this comment.
@ipa-bfb
Needs explanation
| @@ -85,6 +85,8 @@ class InverseDifferentialKinematicsSolver | |||
|
|
|||
| /** CartToJnt for chain using SVD considering KinematicExtensions and various DampingMethods **/ | |||
| virtual int CartToJnt(const JointStates& joint_states, | |||
| const geometry_msgs::Pose pose, | |||
| const KDL::Twist& twist, | |||
There was a problem hiding this comment.
@ipa-bfb
What are the additional arguments used for? Where are they used?
Disslike modification of BaseClass!
| @@ -51,7 +51,7 @@ class KinematicExtensionBase | |||
|
|
|||
| virtual bool initExtension() = 0; | |||
| virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain) = 0; | |||
| virtual JointStates adjustJointStates(const JointStates& joint_states) = 0; | |||
| virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) = 0; | |||
There was a problem hiding this comment.
Disslike modification of BaseClass!
| @@ -142,7 +144,8 @@ bool CobTwistController::initialize() | |||
| { | |||
| twist_controller_params_.frame_names.push_back(chain_.getSegment(i).getName()); | |||
| } | |||
| register_link_client_ = nh_.serviceClient<cob_srvs::SetString>("obstacle_distance/registerLinkOfInterest"); | |||
|
|
|||
| register_link_client_ = nh_.serviceClient<cob_srvs::SetString>("/register_links"); | |||
There was a problem hiding this comment.
This is clearly related to #101 - which is still WIP
Try to separate everything related to this feature from the TwistController-Extensions feature, i.e. JointSingularityAvoidance
| @@ -164,7 +167,7 @@ bool CobTwistController::initialize() | |||
| ros::Duration(1.0).sleep(); | |||
|
|
|||
| /// initialize ROS interfaces | |||
| obstacle_distance_sub_ = nh_.subscribe("obstacle_distance", 1, &CallbackDataMediator::distancesToObstaclesCallback, &callback_data_mediator_); | |||
| obstacle_distance_sub_ = nh_.subscribe("/obstacle_distances", 1, &CallbackDataMediator::distancesToObstaclesCallback, &callback_data_mediator_); | |||
| @@ -309,7 +315,7 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo | |||
| { | |||
| if (!register_link_client_.exists()) | |||
| { | |||
| ROS_ERROR("ServiceServer 'obstacle_distance/registerLinkOfInterest' does not exist. CA not possible"); | |||
| ROS_ERROR("ServiceServer '/register_links' does not exist. CA not possible"); | |||
| } | ||
| result = svd_V * S * svd.matrixU().transpose(); | ||
| break; | ||
| } |
There was a problem hiding this comment.
@ipa-bfb
Please provide link to literature! Where do these formulae come from?
|
|
||
| result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose(); | ||
| break; | ||
| } |
There was a problem hiding this comment.
@ipa-bfb
Please provide link to literature! Where do these formulae come from?
| singularValuesInv(i) = (singularValues(i) < eps_truncation) ? 0.0 : singularValues(i) / denominator; | ||
| } | ||
|
|
||
| result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose(); |
There was a problem hiding this comment.
@ipa-bfb
Please provide link to literature! Where do these formulae come from?
| @@ -54,7 +54,8 @@ class IPseudoinverseCalculator | |||
| */ | |||
| virtual Eigen::MatrixXd calculate(const TwistControllerParams& params, | |||
| boost::shared_ptr<DampingBase> db, | |||
| const Eigen::MatrixXd& jacobian) const = 0; | |||
| const Eigen::MatrixXd& jacobian, | |||
| const JointStates& joint_states) const = 0; | |||
There was a problem hiding this comment.
Disslike modification of BaseClass!
|
In particular, I disslike the API-changes of: |
fmessmer
force-pushed
the
twist_controller_extensions
branch
from
12acf87
to
fb0569d
Compare
|
Well, I definitely need input from @ipa-bfb here! However, I am closing other WIP-PRs related to TwistControlExtensions as they will be included in this one:
|
|
@ipa-fxm I developed a new method for singularity avoidance and joint limit avoidance. @ipa-fxm-cm helped me implementing this new features. The singularity avoidance was working fine and the joint limit avoidance needs further work. @ipa-fxm-cm was trying some methods to optimise the manipulability based on a paper btu as far as I know the results were not satisfying. And so, connected to the implementation of these new methods we have introduced all the changes in #121. This was not the way of doing it so I recommend to not merge this changes. I will send you an invitation for a meeting to discuss a good implementation of this new method. |
|
As discussed, @ipa-bfb will provide new PRs for his new SA and JLA methods.... |
fmessmer
changed the title
|
I'll close this as it is unmergable and most features of this PR are WIP/untested/superseded... |
rebase of #110
Do not merge yet!
I'm still trying to understand what extensions have been added, whether they work correctly as well as whether they are worth keeping