goal_minimize_change.cpp
Go to the documentation of this file.
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 


constrained_ik
Author(s): Chris Lewis , Jeremy Zoss , Dan Solomon
autogenerated on Mon Oct 6 2014 00:52:26