00001 /* 00002 * Software License Agreement (Apache License) 00003 * 00004 * Copyright (c) 2013, Southwest Research Institute 00005 * 00006 * Licensed under the Apache License, Version 2.0 (the "License"); 00007 * you may not use this file except in compliance with the License. 00008 * You may obtain a copy of the License at 00009 * 00010 * http://www.apache.org/licenses/LICENSE-2.0 00011 * 00012 * Unless required by applicable law or agreed to in writing, software 00013 * distributed under the License is distributed on an "AS IS" BASIS, 00014 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00015 * See the License for the specific language governing permissions and 00016 * limitations under the License. 00017 */ 00018 00019 #include "constrained_ik/constrained_ik.h" 00020 #include "constrained_ik/constraints/goal_mid_joint.h" 00021 00022 namespace constrained_ik 00023 { 00024 namespace constraints 00025 { 00026 00027 using namespace Eigen; 00028 00029 // initialize limits/tolerances to default values 00030 GoalMidJoint::GoalMidJoint() : Constraint(), weight_(1.0) 00031 { 00032 } 00033 00034 Eigen::VectorXd GoalMidJoint::calcError() 00035 { 00036 VectorXd err = mid_range_ - state_.joints; 00037 err *= weight_; 00038 return err; 00039 } 00040 00041 Eigen::MatrixXd GoalMidJoint::calcJacobian() 00042 { 00043 size_t n = state_.joints.size(); // number of joints 00044 MatrixXd J = MatrixXd::Identity(n,n) * weight_; 00045 return J; 00046 } 00047 00048 void GoalMidJoint::init(const Constrained_IK *ik) 00049 { 00050 Constraint::init(ik); 00051 00052 // initialize joint/thresholding limits 00053 MatrixXd joint_limits = ik->getKin().getLimits(); 00054 mid_range_ = joint_limits.col(1) - joint_limits.col(0); 00055 } 00056 00057 } // namespace constraints 00058 } // namespace constrained_ik 00059