hpp::model::SO3JointConfig Class Reference

Configuration of a JointSO3. More...

#include <hpp/model/joint-configuration.hh>

Inheritance diagram for hpp::model::SO3JointConfig:
Collaboration diagram for hpp::model::SO3JointConfig:

Public Member Functions

 SO3JointConfig ()
 
virtual ~SO3JointConfig ()
 
virtual void interpolate (ConfigurationIn_t q1, ConfigurationIn_t q2, const value_type &u, const size_type &index, ConfigurationOut_t result)
 Interpolate two configurations of the joint. More...
 
virtual value_type distance (ConfigurationIn_t q1, ConfigurationIn_t q2, const size_type &index) const
 Distance between two configurations of the joint. More...
 
virtual void integrate (ConfigurationIn_t q, vectorIn_t v, const size_type &indexConfig, const size_type &indexVelocity, ConfigurationOut_t result) const
 Integrate constant derivative during unit time. More...
 
virtual void difference (ConfigurationIn_t q1, ConfigurationIn_t q2, const size_type &indexConfig, const size_type &indexVelocity, vectorOut_t result) const
 Difference between two configurations. More...
 
virtual void uniformlySample (const size_type &index, ConfigurationOut_t result) const
 Uniformly sample the configuration space of the joint. More...
 
- Public Member Functions inherited from hpp::model::JointConfiguration
 JointConfiguration (size_type configSize)
 Constructor. More...
 
virtual ~JointConfiguration ()
 Destructor. More...
 
void isBounded (size_type rank, bool bounded)
 
bool isBounded (size_type rank) const
 Get whether given degree of freedom is bounded. More...
 
value_type lowerBound (size_type rank) const
 Get lower bound of given degree of freedom. More...
 
value_type upperBound (size_type rank) const
 Get upper bound of given degree of freedom. More...
 
void lowerBound (size_type rank, value_type lowerBound)
 Set lower bound of given degree of freedom. More...
 
void upperBound (size_type rank, value_type upperBound)
 Set upper bound of given degree of freedom. More...
 

Detailed Description

Configuration of a JointSO3.

Constructor & Destructor Documentation

◆ SO3JointConfig()

hpp::model::SO3JointConfig::SO3JointConfig ( )

◆ ~SO3JointConfig()

virtual hpp::model::SO3JointConfig::~SO3JointConfig ( )
virtual

Member Function Documentation

◆ difference()

virtual void hpp::model::SO3JointConfig::difference ( ConfigurationIn_t  q1,
ConfigurationIn_t  q2,
const size_type indexConfig,
const size_type indexVelocity,
vectorOut_t  result 
) const
virtual

Difference between two configurations.

Parameters
q1configuration,
q2configuration,
indexConfigindex of first component of q corresponding to the joint.
indexVelocityindex of first component of v corresponding to the joint
Return values
result[index:index+joint number dof] part of vector representing the difference between q1 and q2.

\[ \texttt{result}[\texttt{index}:\texttt{index}+3] = \textbf{q}_1[\texttt{index}:\texttt{index}+3] - \textbf{q}_2[\texttt{index}:\texttt{index}+3] \]

The difference is computed as follows:

\[ \textbf{q}_1 [\texttt{index}+3:\texttt{index}+6] = \exp \left(\texttt{result}[\texttt{index}+3:\texttt{index}+6]_{\times} \right)\textbf{q}_2 [\texttt{index}+3:\texttt{index}+6] \]

Implements hpp::model::JointConfiguration.

◆ distance()

virtual value_type hpp::model::SO3JointConfig::distance ( ConfigurationIn_t  q1,
ConfigurationIn_t  q2,
const size_type index 
) const
virtual

Distance between two configurations of the joint.

Parameters
q1,q2two configurations of the robot
indexindex of first component of q1 and q2 corresponding to the joint.
Returns
the angle between the joint orientations

Implements hpp::model::JointConfiguration.

◆ integrate()

virtual void hpp::model::SO3JointConfig::integrate ( ConfigurationIn_t  q,
vectorIn_t  v,
const size_type indexConfig,
const size_type indexVelocity,
ConfigurationOut_t  result 
) const
virtual

Integrate constant derivative during unit time.

Parameters
qinitial configuration
vjoint velocity
indexConfigindex of first component of q corresponding to the joint.
indexVelocityindex of first component of v corresponding to the joint
Return values
resultwrite joint configuration in result [indexConfig:indexConfig + joint config size]
Note
if result is beying bounds, return active bound.

Implements hpp::model::JointConfiguration.

◆ interpolate()

virtual void hpp::model::SO3JointConfig::interpolate ( ConfigurationIn_t  q1,
ConfigurationIn_t  q2,
const value_type u,
const size_type index,
ConfigurationOut_t  result 
)
virtual

Interpolate two configurations of the joint.

Parameters
q1,q2,twoconfigurations to interpolate
uin [0,1] position along the interpolation: q1 for u=0, q2 for u=1
indexindex of first component of q1 and q2 corresponding to the joint.
Return values
resultwrite joint configuration in result [index:index+nb dofs]

q1 and q2 are configurations of the robot where coordinates between index and index + number of dofs - 1 correspond to the configuration of the joint:

  • a real value for translation joint and bounded rotation joints,
  • an angle for unbounded rotation joints,
  • x, y, z, roll, pitch, yaw for freeflyer joints.

Implements hpp::model::JointConfiguration.

◆ uniformlySample()

virtual void hpp::model::SO3JointConfig::uniformlySample ( const size_type index,
ConfigurationOut_t  result 
) const
virtual

Uniformly sample the configuration space of the joint.

Parameters
indexindex of first component of q corresponding to the joint.
Return values
resultwrite joint configuration in result [index:index+nb dofs]

Implements hpp::model::JointConfiguration.