joint_vel_limits.h

Go to the documentation of this file.

#ifndef JOINT_VEL_LIMITS_H_
#define JOINT_VEL_LIMITS_H_

#include "constrained_ik/constraint.h"
#include <vector>

namespace constrained_ik
{
namespace constraints
{
class JointVelLimits: public Constraint
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  struct JointVelLimitsData: public ConstraintData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    const constraints::JointVelLimits* parent_; 
    std::vector<int> limited_joints_;  
    Eigen::VectorXd jvel_; 
    JointVelLimitsData(const constrained_ik::SolverState &state, const constraints::JointVelLimits* parent);
  };

  JointVelLimits();

  void init(const Constrained_IK* ik) override;

  constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override;

  void loadParameters(const XmlRpc::XmlRpcValue &constraint_xml) override;

  virtual Eigen::MatrixXd calcJacobian(const JointVelLimitsData &cdata) const;

  virtual Eigen::VectorXd calcError(const JointVelLimitsData &cdata) const;

  virtual bool checkStatus(const JointVelLimitsData &cdata) const { return cdata.limited_joints_.size() == 0; }

  virtual double getWeight() const {return weight_;}

  virtual void setWeight(const double &weight) {weight_ = weight;}

  virtual double getTimestep() const {return timestep_;}

  virtual void setTimestep(const double &timestep) {timestep_ = timestep;}

protected:
  Eigen::VectorXd vel_limits_; 
  double weight_;  
  double timestep_; 
};

} /* namespace constraints */
} /* namespace constrained_ik */
#endif /* JOINT_VEL_LIMITS_H_ */