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