orwell 0.0.0
orwell::CartesianImpedanceController< dof, Reference > Class Template Referenceabstract

base class for an impedance controller derived from TorqueController More...

#include <CartesianImpedanceController.hpp>

Inheritance diagram for orwell::CartesianImpedanceController< dof, Reference >:
orwell::ReferenceController< Reference, DerivedController >

Public Member Functions

 CartesianImpedanceController (const sackmesser::Interface::Ptr &interface, const std::string &name)
 loads configuration parameters
 
RobotState< dof >::Vector computeCommand ()
 implements the impedance control law
 
- Public Member Functions inherited from orwell::ReferenceController< Reference, DerivedController >
 ReferenceController (const sackmesser::Interface::Ptr &interface, const std::string &name)
 
void setReference (const Reference &reference)
 set Reference
 
const Reference & getReference () const
 provide access to the Reference to derived classes
 

Protected Member Functions

void setPositionError (const Eigen::Vector< double, 6 > &position_error)
 set the value of position_error_
 
void setVelocityError (const Eigen::Vector< double, 6 > &velocity_error)
 set the value of velocity_error_
 
void setAccelerationError (const Eigen::Vector< double, 6 > &acceleration_error)
 set the value of acceleration_error_
 
Eigen::Vector< double, 6 > limitWrench (const Eigen::Vector< double, 6 > &wrench)
 limit the end-effector wrench to the given wrench_limit_
 
virtual void computeStateError ()=0
 computes the state error for the impedance control law
 

Additional Inherited Members

- Public Types inherited from orwell::ReferenceController< Reference, DerivedController >
using Ptr = std::shared_ptr<ReferenceController>
 typedef
 

Detailed Description

template<int dof, class Reference>
class orwell::CartesianImpedanceController< dof, Reference >

base class for an impedance controller derived from TorqueController

Template Parameters
dofnumber of degrees of freedom of the controlled system
Referencereference target

Definition at line 35 of file CartesianImpedanceController.hpp.

Constructor & Destructor Documentation

◆ CartesianImpedanceController()

template<int dof, class Reference >
orwell::CartesianImpedanceController< dof, Reference >::CartesianImpedanceController ( const sackmesser::Interface::Ptr & interface,
const std::string & name )

loads configuration parameters

Parameters
interfaceinterfaces the configuration server
nameparameter namespace

Definition at line 30 of file CartesianImpedanceController.hxx.

31 : TorqueReferenceController<dof, Reference>(interface, name)
32 {
33 config_ = interface->getConfigurations()->load<Configuration>(name);
34
35 position_error_ = Eigen::Vector<double, 6>::Zero();
36 velocity_error_ = Eigen::Vector<double, 6>::Zero();
37 acceleration_error_ = Eigen::Vector<double, 6>::Zero();
38 }

Member Function Documentation

◆ computeCommand()

template<int dof, class Reference >
RobotState< dof >::Vector orwell::CartesianImpedanceController< dof, Reference >::computeCommand ( )

implements the impedance control law

derived classes should call setPositionError, setVelocityError and setAccelerationError during computeStateError

Returns
joint torque control command

Definition at line 66 of file CartesianImpedanceController.hxx.

67 {
69
70 return this->getRobotModel()->getJacobian().transpose() * limitWrench(this->getDesiredWrench());
71 }
Eigen::Vector< double, 6 > limitWrench(const Eigen::Vector< double, 6 > &wrench)
limit the end-effector wrench to the given wrench_limit_
virtual void computeStateError()=0
computes the state error for the impedance control law

References orwell::CartesianImpedanceController< dof, Reference >::computeStateError(), and orwell::CartesianImpedanceController< dof, Reference >::limitWrench().

◆ computeStateError()

template<int dof, class Reference >
virtual void orwell::CartesianImpedanceController< dof, Reference >::computeStateError ( )
protectedpure virtual

computes the state error for the impedance control law

derived classes should call setPositionError, setVelocityError, setAccelerationError

Referenced by orwell::CartesianImpedanceController< dof, Reference >::computeCommand().

◆ limitWrench()

template<int dof, class Reference >
Eigen::Vector< double, 6 > orwell::CartesianImpedanceController< dof, Reference >::limitWrench ( const Eigen::Vector< double, 6 > & wrench)
protected

limit the end-effector wrench to the given wrench_limit_

Parameters
wrenchwrench computed by getDesiredWrench
Returns
wrench within the limits

Definition at line 100 of file CartesianImpedanceController.hxx.

101 {
102 return wrench.array().min(config_.wrench_limits_.array()).max(-config_.wrench_limits_.array());
103 }

Referenced by orwell::CartesianImpedanceController< dof, Reference >::computeCommand().

◆ setAccelerationError()

template<int dof, class Reference >
void orwell::CartesianImpedanceController< dof, Reference >::setAccelerationError ( const Eigen::Vector< double, 6 > & acceleration_error)
protected

set the value of acceleration_error_

derived classes should call this function during computeStateError

Parameters
acceleration_errorthe acceleration error for the impedance control law

Definition at line 94 of file CartesianImpedanceController.hxx.

95 {
96 acceleration_error_ = acceleration_error;
97 }

◆ setPositionError()

template<int dof, class Reference >
void orwell::CartesianImpedanceController< dof, Reference >::setPositionError ( const Eigen::Vector< double, 6 > & position_error)
protected

set the value of position_error_

derived classes should call this function during computeStateError

Parameters
position_errorthe position error for the impedance control law

Definition at line 82 of file CartesianImpedanceController.hxx.

83 {
84 position_error_ = position_error;
85 }

◆ setVelocityError()

template<int dof, class Reference >
void orwell::CartesianImpedanceController< dof, Reference >::setVelocityError ( const Eigen::Vector< double, 6 > & velocity_error)
protected

set the value of velocity_error_

derived classes should call this function during computeStateError

Parameters
velocity_errorthe velocity error for the impedance control law

Definition at line 88 of file CartesianImpedanceController.hxx.

89 {
90 velocity_error_ = velocity_error;
91 }

The documentation for this class was generated from the following files: