include
cartesian_trajectory_controller
ik_solver_example.h
Go to the documentation of this file.
1
// -- BEGIN LICENSE BLOCK ----------------------------------------------
2
// Copyright 2021 FZI Forschungszentrum Informatik
3
// Created on behalf of Universal Robots A/S
4
//
5
// Licensed under the Apache License, Version 2.0 (the "License");
6
// you may not use this file except in compliance with the License.
7
// You may obtain a copy of the License at
8
//
9
// http://www.apache.org/licenses/LICENSE-2.0
10
//
11
// Unless required by applicable law or agreed to in writing, software
12
// distributed under the License is distributed on an "AS IS" BASIS,
13
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14
// See the License for the specific language governing permissions and
15
// limitations under the License.
16
// -- END LICENSE BLOCK ------------------------------------------------
17
18
//-----------------------------------------------------------------------------
25
//-----------------------------------------------------------------------------
26
27
#pragma once
28
29
#include <
cartesian_trajectory_controller/ik_solver_base.h
>
30
#include <kdl/chainiksolverpos_lma.hpp>
31
#include <kdl/chain.hpp>
32
#include <memory>
33
34
namespace
ros_controllers_cartesian
35
{
42
class
ExampleIKSolver
:
public
IKSolver
43
{
44
public
:
45
ExampleIKSolver
(){};
46
~ExampleIKSolver
(){};
47
54
bool
init
(
const
KDL::Chain& robot_chain,
ros::NodeHandle
&,
ros::NodeHandle
&)
override
55
{
56
robot_chain_
= robot_chain;
57
lma_solver_
= std::make_unique<KDL::ChainIkSolverPos_LMA>(
robot_chain_
);
58
return
true
;
59
};
60
65
virtual
int
cartToJnt
(
const
KDL::JntArray& q_init,
const
KDL::Frame& goal, KDL::JntArray& q_out)
override
66
{
67
return
lma_solver_
->CartToJnt(q_init, goal, q_out);
68
};
69
70
private
:
71
std::unique_ptr<KDL::ChainIkSolverPos_LMA>
lma_solver_
;
72
KDL::Chain
robot_chain_
;
73
};
74
}
// namespace ros_controllers_cartesian
ros_controllers_cartesian::ExampleIKSolver::robot_chain_
KDL::Chain robot_chain_
Definition:
ik_solver_example.h:72
ros_controllers_cartesian
ros_controllers_cartesian::ExampleIKSolver::ExampleIKSolver
ExampleIKSolver()
Definition:
ik_solver_example.h:45
ros_controllers_cartesian::ExampleIKSolver::init
bool init(const KDL::Chain &robot_chain, ros::NodeHandle &, ros::NodeHandle &) override
Initialize the solver.
Definition:
ik_solver_example.h:54
ros_controllers_cartesian::ExampleIKSolver::cartToJnt
virtual int cartToJnt(const KDL::JntArray &q_init, const KDL::Frame &goal, KDL::JntArray &q_out) override
Compute Inverse Kinematics with KDL's Levenberg Marquardt solver.
Definition:
ik_solver_example.h:65
ik_solver_base.h
ros_controllers_cartesian::IKSolver
Base class for Inverse Kinematics (IK) solvers.
Definition:
ik_solver_base.h:46
ros_controllers_cartesian::ExampleIKSolver::~ExampleIKSolver
~ExampleIKSolver()
Definition:
ik_solver_example.h:46
ros_controllers_cartesian::ExampleIKSolver
A wrapper around KDL's Levenberg Marquardt solver.
Definition:
ik_solver_example.h:42
ros::NodeHandle
ros_controllers_cartesian::ExampleIKSolver::lma_solver_
std::unique_ptr< KDL::ChainIkSolverPos_LMA > lma_solver_
Definition:
ik_solver_example.h:68
cartesian_trajectory_controller
Author(s):
autogenerated on Tue Oct 15 2024 02:09:16