Skip to content

Commit

Permalink
[walk_interface] Add interface for hand trajectories.
Browse files Browse the repository at this point in the history
  • Loading branch information
@thomas-moulard
thomas-moulard committed Apr 25, 2012
1 parent db69b34 commit 0ebbac5
Show file tree
Hide file tree
Showing 2 changed files with 214 additions and 3 deletions.
73 changes: 72 additions & 1 deletion walk_interfaces/include/walk_interfaces/pattern-generator.hh
View file
Expand Up @@ -102,6 +102,22 @@ namespace walk
const Vector3d& centerOfMass,
const Posture& posture);

/// \brief Define the initial robot state.
///
/// \param leftFoot left foot initial position.
/// \param rightFoot right foot initial position.
/// \param centerOfMass center of mass initial position.
/// \param posture initial posture.
/// \param leftHand left hand initial position.
/// \param rightHand right hand initial position.
void setInitialRobotPosition(const HomogeneousMatrix3d& leftFoot,
const HomogeneousMatrix3d& rightFoot,
const Vector3d& centerOfMass,
const Posture& posture,
const HomogeneousMatrix3d& leftHand,
const HomogeneousMatrix3d& rightHand);


/// \brief Define the final robot state.
///
/// \param leftFoot left foot final position.
Expand All @@ -113,6 +129,22 @@ namespace walk
const Vector3d& centerOfMass,
const Posture& posture);

/// \brief Define the final robot state.
///
/// \param leftFoot left foot final position.
/// \param rightFoot right foot final position.
/// \param centerOfMass center of mass final position.
/// \param posture final posture.
/// \param leftHand left hand final position.
/// \param rightHand right hand final position.
void setFinalRobotPosition(const HomogeneousMatrix3d& leftFoot,
const HomogeneousMatrix3d& rightFoot,
const Vector3d& centerOfMass,
const Posture& posture,
const HomogeneousMatrix3d& leftHand,
const HomogeneousMatrix3d& rightHand);


/// \brief Set the footprint sequence for the pattern generator.
///
/// \param startWithLeftFoot True if the movement first flying
Expand Down Expand Up @@ -140,6 +172,11 @@ namespace walk
/// \brief Posture trajectory getter.
const TrajectoryNd& postureTrajectory() const;

/// \brief Left hand trajectory getter.
const Trajectory3d& leftHandTrajectory() const;
/// \brief Right hand trajectory getter.
const Trajectory3d& rightHandTrajectory() const;

/// \}

/// \name Initial configuration.
Expand All @@ -153,6 +190,10 @@ namespace walk
const Vector3d& initialCenterOfMassPosition() const;
/// \brief Initial posture getter.
const Posture& initialPosture() const;
/// \brief Initial left hand position getter.
const HomogeneousMatrix3d& initialLeftHandPosition() const;
/// \brief Initial right hand position getter.
const HomogeneousMatrix3d& initialRightHandPosition() const;

/// \}

Expand All @@ -168,6 +209,10 @@ namespace walk
const Vector3d& finalCenterOfMassPosition() const;
/// \brief Final posture getter.
const Posture& finalPosture() const;
/// \brief Final left hand position getter.
const HomogeneousMatrix3d& finalLeftHandPosition() const;
/// \brief Final right hand position getter.
const HomogeneousMatrix3d& finalRightHandPosition() const;

/// \}

Expand All @@ -182,6 +227,10 @@ namespace walk
TrajectoryV3d& getCenterOfMassTrajectory();
/// \brief Posture trajectory setter.
TrajectoryNd& getPostureTrajectory();
/// \brief Left hand trajectory setter.
Trajectory3d& getLeftHandTrajectory();
/// \brief Right hand trajectory setter.
Trajectory3d& getRightHandTrajectory();

/// \brief Initial left foot position setter.
HomogeneousMatrix3d& getInitialLeftFootPosition();
Expand All @@ -191,6 +240,10 @@ namespace walk
Vector3d& getInitialCenterOfMassPosition();
/// \brief Initial posture setter.
Posture& getInitialPosture();
/// \brief Initial left hand position setter.
HomogeneousMatrix3d& getInitialLeftHandPosition();
/// \brief Initial right hand position setter.
HomogeneousMatrix3d& getInitialRightHandPosition();

/// \brief Final left foot position setter.
HomogeneousMatrix3d& getFinalLeftFootPosition();
Expand All @@ -200,6 +253,11 @@ namespace walk
Vector3d& getFinalCenterOfMassPosition();
/// \brief Final posture setter.
Posture& getFinalPosture();
/// \brief Final left hand position setter.
HomogeneousMatrix3d& getFinalLeftHandPosition();
/// \brief Final right hand position setter.
HomogeneousMatrix3d& getFinalRightHandPosition();


/// \brief Compute the reference trajectories.
///
Expand All @@ -224,6 +282,11 @@ namespace walk
TrajectoryV2d zmpTrajectory_;
/// \brief Posture trajectory.
TrajectoryNd postureTrajectory_;
/// \brief Left hand trajectory.
Trajectory3d leftHandTrajectory_;
/// \brief Right hand trajectory.
Trajectory3d rightHandTrajectory_;


/// \brief Initial left foot position.
HomogeneousMatrix3d initialLeftFootPosition_;
Expand All @@ -233,6 +296,10 @@ namespace walk
Vector3d initialCenterOfMassPosition_;
/// \brief Initial posture.
Posture initialPosture_;
/// \brief Initial left hand position.
HomogeneousMatrix3d initialLeftHandPosition_;
/// \brief Initial right hand position.
HomogeneousMatrix3d initialRightHandPosition_;

/// \brief Final left foot position.
HomogeneousMatrix3d finalLeftFootPosition_;
Expand All @@ -242,6 +309,10 @@ namespace walk
Vector3d finalCenterOfMassPosition_;
/// \brief Final posture.
Posture finalPosture_;
/// \brief Final left hand position.
HomogeneousMatrix3d finalLeftHandPosition_;
/// \brief Final right hand position.
HomogeneousMatrix3d finalRightHandPosition_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
Expand Down Expand Up @@ -274,7 +345,7 @@ namespace walk
typedef DiscretizedTrajectory<StampedVectorNd> TrajectoryNd;
};


class DiscretizedPatternGenerator2d
: public PatternGenerator<DiscretizedPatternGenerator2d>
{};
Expand Down
144 changes: 142 additions & 2 deletions walk_interfaces/include/walk_interfaces/pattern-generator.hxx
View file
Expand Up @@ -13,6 +13,8 @@ namespace walk
centerOfMassTrajectory_(),
zmpTrajectory_(),
postureTrajectory_(),
leftHandTrajectory_(),
rightHandTrajectory_(),

initialLeftFootPosition_(),
initialRightFootPosition_(),
Expand All @@ -28,11 +30,15 @@ namespace walk
initialRightFootPosition_.setIdentity();
initialCenterOfMassPosition_.setZero();
initialPosture_.setZero();
initialLeftHandPosition_.setIdentity();
initialRightHandPosition_.setIdentity();

finalLeftFootPosition_.setIdentity();
finalRightFootPosition_.setIdentity();
finalCenterOfMassPosition_.setZero();
finalPosture_.setZero();
finalLeftHandPosition_.setIdentity();
finalRightHandPosition_.setIdentity();
}

template <typename T>
Expand All @@ -44,7 +50,9 @@ namespace walk
rightFootTrajectory_(pg.rightFootTrajectory_),
centerOfMassTrajectory_(pg.centerOfMassTrajectory_),
zmpTrajectory_(pg.zmpTrajectory_),
postureTrajectory_(pg.postureTrajectory_)
postureTrajectory_(pg.postureTrajectory_),
leftHandTrajectory_(pg.leftHandTrajectory_),
rightHandTrajectory_(pg.rightHandTrajectory_)
{}

template <typename T>
Expand All @@ -61,6 +69,8 @@ namespace walk
this->centerOfMassTrajectory_ = pg.centerOfMassTrajectory_;
this->zmpTrajectory_ = pg.zmpTrajectory_;
this->postureTrajectory_ = pg.postureTrajectory_;
this->leftHandTrajectory_ = pg.leftHandTrajectory_;
this->rightHandTrajectory_ = pg.rightHandTrajectory_;
return *this;
}

Expand All @@ -78,6 +88,25 @@ namespace walk
initialPosture_ = posture;
}

template <typename T>
void
PatternGenerator<T>::setInitialRobotPosition
(const HomogeneousMatrix3d& leftFoot,
const HomogeneousMatrix3d& rightFoot,
const Vector3d& centerOfMass,
const Posture& posture,
const HomogeneousMatrix3d& leftHand,
const HomogeneousMatrix3d& rightHand)
{
initialLeftFootPosition_ = leftFoot;
initialRightFootPosition_ = rightFoot;
initialCenterOfMassPosition_ = centerOfMass;
initialPosture_ = posture;
initialLeftHandPosition_ = leftHand;
initialRightHandPosition_ = rightHand;
}


template <typename T>
void
PatternGenerator<T>::setFinalRobotPosition
Expand All @@ -92,6 +121,25 @@ namespace walk
finalPosture_ = posture;
}

template <typename T>
void
PatternGenerator<T>::setFinalRobotPosition
(const HomogeneousMatrix3d& leftFoot,
const HomogeneousMatrix3d& rightFoot,
const Vector3d& centerOfMass,
const Posture& posture,
const HomogeneousMatrix3d& leftHand,
const HomogeneousMatrix3d& rightHand)
{
finalLeftFootPosition_ = leftFoot;
finalRightFootPosition_ = rightFoot;
finalCenterOfMassPosition_ = centerOfMass;
finalPosture_ = posture;
finalLeftHandPosition_ = leftHand;
finalRightHandPosition_ = rightHand;
}



template <typename T>
const typename PatternGenerator<T>::footprints_t&
Expand Down Expand Up @@ -156,6 +204,20 @@ namespace walk
return postureTrajectory_;
}

template <typename T>
const typename PatternGenerator<T>::Trajectory3d&
PatternGenerator<T>::leftHandTrajectory() const
{
return leftHandTrajectory_;
}

template <typename T>
const typename PatternGenerator<T>::Trajectory3d&
PatternGenerator<T>::rightHandTrajectory() const
{
return rightHandTrajectory_;
}

template <typename T>
typename PatternGenerator<T>::Trajectory3d&
PatternGenerator<T>::getLeftFootTrajectory()
Expand Down Expand Up @@ -191,6 +253,20 @@ namespace walk
return postureTrajectory_;
}

template <typename T>
typename PatternGenerator<T>::Trajectory3d&
PatternGenerator<T>::getLeftHandTrajectory()
{
return leftHandTrajectory_;
}

template <typename T>
typename PatternGenerator<T>::Trajectory3d&
PatternGenerator<T>::getRightHandTrajectory()
{
return rightHandTrajectory_;
}

template <typename T>
HomogeneousMatrix3d&
PatternGenerator<T>::getInitialLeftFootPosition()
Expand Down Expand Up @@ -219,6 +295,21 @@ namespace walk
return initialPosture_;
}

template <typename T>
HomogeneousMatrix3d&
PatternGenerator<T>::getInitialLeftHandPosition()
{
return initialLeftHandPosition_;
}

template <typename T>
HomogeneousMatrix3d&
PatternGenerator<T>::getInitialRightHandPosition()
{
return initialRightHandPosition_;
}


template <typename T>
HomogeneousMatrix3d&
PatternGenerator<T>::getFinalLeftFootPosition()
Expand Down Expand Up @@ -247,6 +338,21 @@ namespace walk
return finalPosture_;
}

template <typename T>
HomogeneousMatrix3d&
PatternGenerator<T>::getFinalLeftHandPosition()
{
return finalLeftHandPosition_;
}

template <typename T>
HomogeneousMatrix3d&
PatternGenerator<T>::getFinalRightHandPosition()
{
return finalRightHandPosition_;
}




template <typename T>
Expand All @@ -263,6 +369,21 @@ namespace walk
return initialRightFootPosition_;
}

template <typename T>
const HomogeneousMatrix3d&
PatternGenerator<T>::initialLeftHandPosition() const
{
return initialLeftHandPosition_;
}

template <typename T>
const HomogeneousMatrix3d&
PatternGenerator<T>::initialRightHandPosition() const
{
return initialRightHandPosition_;
}


template <typename T>
const Vector3d&
PatternGenerator<T>::initialCenterOfMassPosition() const
Expand Down Expand Up @@ -305,6 +426,21 @@ namespace walk
return finalPosture_;
}

template <typename T>
const HomogeneousMatrix3d&
PatternGenerator<T>::finalLeftHandPosition() const
{
return finalLeftHandPosition_;
}

template <typename T>
const HomogeneousMatrix3d&
PatternGenerator<T>::finalRightHandPosition() const
{
return finalRightHandPosition_;
}


template <typename T>
std::ostream&
operator<<(std::ostream& os, const PatternGenerator<T>& pg)
Expand Down Expand Up @@ -335,7 +471,11 @@ namespace walk
<< "zmp trajectory:\n"
<< pg.zmpTrajectory()
<< "center of mass trajectory:\n"
<< pg.centerOfMassTrajectory();
<< pg.centerOfMassTrajectory()
<< "left hand trajectory:\n"
<< pg.leftHandTrajectory()
<< "right hand trajectory:\n"
<< pg.rightHandTrajectory();
return os;
}
} // end of namespace walk.
Expand Down

0 comments on commit 0ebbac5

Please sign in to comment.