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_minimize_change.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 GoalMinimizeChange::GoalMinimizeChange() : Constraint(), weight_(1.0) 00031 { 00032 } 00033 00034 Eigen::VectorXd GoalMinimizeChange::calcError() 00035 { 00036 VectorXd err = state_.joint_seed - state_.joints; 00037 err *= weight_; 00038 return err; 00039 } 00040 00041 Eigen::MatrixXd GoalMinimizeChange::calcJacobian() 00042 { 00043 size_t n = numJoints(); // number of joints 00044 MatrixXd J = MatrixXd::Identity(n,n) * weight_; 00045 return J; 00046 } 00047 00048 } // namespace constraints 00049 } // namespace constrained_ik 00050