Constraint to push joint to center of its range. More...
#include <goal_mid_joint.h>
Public Member Functions | |
virtual Eigen::VectorXd | calcError (const ConstraintData &cdata) const |
Desired joint velocity is difference between min-range and current position. | |
virtual Eigen::MatrixXd | calcJacobian (const ConstraintData &cdata) const |
Jacobian is identity because all joints are affected. | |
virtual bool | checkStatus (const ConstraintData &cdata) const |
Termination criteria for mid-joint constraint. | |
constrained_ik::ConstraintResults | evalConstraint (const SolverState &state) const override |
see base class for documentation | |
virtual double | getWeight () const |
Getter for weight_. | |
void | init (const Constrained_IK *ik) override |
Initialize constraint (overrides Constraint::init) Initializes internal variable representing mid-range of each joint Should be called before using class. | |
void | loadParameters (const XmlRpc::XmlRpcValue &constraint_xml) override |
see base class for documentation | |
virtual void | setWeight (double weight) |
setter for weight_ | |
Protected Attributes | |
Eigen::VectorXd | mid_range_ |
mid-range of each joint | |
double | weight_ |
weights used to scale the jocabian and error |
Constraint to push joint to center of its range.
Definition at line 42 of file goal_mid_joint.h.
Eigen::VectorXd constrained_ik::constraints::GoalMidJoint::calcError | ( | const ConstraintData & | cdata | ) | const [virtual] |
Desired joint velocity is difference between min-range and current position.
cdata | The constraint specific data. |
Definition at line 57 of file goal_mid_joint.cpp.
Eigen::MatrixXd constrained_ik::constraints::GoalMidJoint::calcJacobian | ( | const ConstraintData & | cdata | ) | const [virtual] |
Jacobian is identity because all joints are affected.
cdata | The constraint specific data. |
Definition at line 64 of file goal_mid_joint.cpp.
virtual bool constrained_ik::constraints::GoalMidJoint::checkStatus | ( | const ConstraintData & | cdata | ) | const [inline, virtual] |
Termination criteria for mid-joint constraint.
cdata | The constraint specific data. |
Definition at line 81 of file goal_mid_joint.h.
virtual double constrained_ik::constraints::GoalMidJoint::getWeight | ( | ) | const [inline, virtual] |
void constrained_ik::constraints::GoalMidJoint::init | ( | const Constrained_IK * | ik | ) | [override, virtual] |
Initialize constraint (overrides Constraint::init) Initializes internal variable representing mid-range of each joint Should be called before using class.
ik | Pointer to Constrained_IK used for base-class init |
Reimplemented from constrained_ik::Constraint.
Definition at line 71 of file goal_mid_joint.cpp.
virtual void constrained_ik::constraints::GoalMidJoint::setWeight | ( | double | weight | ) | [inline, virtual] |
setter for weight_
weight | Value to set weight_ to |
Definition at line 92 of file goal_mid_joint.h.