00001 00026 #include "constrained_ik/constrained_ik.h" 00027 #include "constrained_ik/constraints/goal_zero_jvel.h" 00028 #include <pluginlib/class_list_macros.h> 00029 PLUGINLIB_EXPORT_CLASS(constrained_ik::constraints::GoalZeroJVel, constrained_ik::Constraint) 00030 00031 namespace constrained_ik 00032 { 00033 namespace constraints 00034 { 00035 00036 using namespace Eigen; 00037 00038 // initialize limits/tolerances to default values 00039 GoalZeroJVel::GoalZeroJVel() : Constraint(), weight_(1.0) 00040 { 00041 } 00042 00043 constrained_ik::ConstraintResults GoalZeroJVel::evalConstraint(const SolverState &state) const 00044 { 00045 constrained_ik::ConstraintResults output; 00046 GoalZeroJVel::ConstraintData cdata(state); 00047 00048 output.error = calcError(cdata); 00049 output.jacobian = calcJacobian(cdata); 00050 output.status = checkStatus(cdata); 00051 00052 return output; 00053 } 00054 00055 Eigen::VectorXd GoalZeroJVel::calcError(const GoalZeroJVel::ConstraintData &cdata) const 00056 { 00057 return VectorXd::Zero(numJoints()); 00058 } 00059 00060 Eigen::MatrixXd GoalZeroJVel::calcJacobian(const GoalZeroJVel::ConstraintData &cdata) const 00061 { 00062 size_t n = numJoints(); // number of joints 00063 MatrixXd J = MatrixXd::Identity(n,n) * weight_; 00064 return J; 00065 } 00066 00067 void GoalZeroJVel::loadParameters(const XmlRpc::XmlRpcValue &constraint_xml) 00068 { 00069 XmlRpc::XmlRpcValue local_xml = constraint_xml; 00070 if (!getParam(local_xml, "weights", weight_)) 00071 { 00072 ROS_WARN("Goal Joint Zero Velocity: Unable to retrieve weights member, default parameter will be used."); 00073 } 00074 00075 if (!getParam(local_xml, "debug", debug_)) 00076 { 00077 ROS_WARN("Goal Joint Zero Velocity: Unable to retrieve debug member, default parameter will be used."); 00078 } 00079 } 00080 00081 } // namespace constraints 00082 } // namespace constrained_ik 00083