-
Notifications
You must be signed in to change notification settings - Fork 4
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' into UnifyCANImplementation
- Loading branch information
Showing
15 changed files
with
487 additions
and
299 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,52 @@ | ||
| #pragma once | ||
|
|
||
| #include "ForwardArmKinematics.h" | ||
| #include "InverseArmKinematics.h" | ||
|
|
||
| #include <memory> | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| namespace kinematics { | ||
|
|
||
| /** | ||
| * @brief A class that combines the forward and inverse kinematics of an arm. | ||
| * | ||
| * @tparam D The number of dimensions of the end effector space. | ||
| * @tparam N The number of joints in the arm. | ||
| */ | ||
| template <unsigned int D, unsigned int N> | ||
| class ArmKinematics : public ForwardArmKinematics<D, N>, public InverseArmKinematics<D, N> { | ||
| public: | ||
| ArmKinematics(std::shared_ptr<const ForwardArmKinematics<D, N>> forwardKin, | ||
| std::shared_ptr<const InverseArmKinematics<D, N>> inverseKin) | ||
| : InverseArmKinematics<D, N>(forwardKin), ik(inverseKin) {} | ||
|
|
||
| bool satisfiesConstraints(const navtypes::Vectord<N>& jointPos) const override { | ||
| return this->fk->satisfiesConstraints(jointPos); | ||
| } | ||
|
|
||
| navtypes::Vectord<N> getSegLens() const override { | ||
| return this->fk->getSegLens(); | ||
| } | ||
|
|
||
| navtypes::Vectord<D> jointPosToEEPos(const navtypes::Vectord<N>& jointPos) const override { | ||
| return this->fk->jointPosToEEPos(jointPos); | ||
| } | ||
|
|
||
| navtypes::Matrixd<D, N> getJacobian(const navtypes::Vectord<N>& jointPos) const override { | ||
| return this->fk->getJacobian(jointPos); | ||
| } | ||
|
|
||
| protected: | ||
| navtypes::Vectord<N> solve(const navtypes::Vectord<D>& eePos, | ||
| const navtypes::Vectord<N>& jointAngles, | ||
| bool& success) const override { | ||
| return ik->eePosToJointPos(eePos, jointAngles, success); | ||
| } | ||
|
|
||
| private: | ||
| const std::shared_ptr<const InverseArmKinematics<D, N>> ik; | ||
| }; | ||
|
|
||
| } // namespace kinematics |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,98 @@ | ||
| #pragma once | ||
|
|
||
| #include "../navtypes.h" | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| namespace kinematics { | ||
|
|
||
| /** | ||
| * @brief A class that calculates the forward kinematics of an arm. | ||
| * | ||
| * @tparam D The number of dimensions of the end effector space. | ||
| * @tparam N The number of joints in the arm. | ||
| */ | ||
| template <unsigned int D, unsigned int N> | ||
| class ForwardArmKinematics { | ||
| public: | ||
| virtual ~ForwardArmKinematics() = default; | ||
|
|
||
| /** | ||
| * @brief Get the number of segments in this arm. | ||
| * | ||
| * @return constexpr unsigned int The number of segments. | ||
| */ | ||
| constexpr unsigned int getNumSegments() const { | ||
| return N; | ||
| } | ||
|
|
||
| /** | ||
| * @brief Get the number of dimensions of the end effector space. | ||
| * | ||
| * @return constexpr unsigned int The number of dimensions. | ||
| */ | ||
| constexpr unsigned int getNumDimensions() const { | ||
| return D; | ||
| } | ||
|
|
||
| /** | ||
| * @brief Get the segment lengths for this arm. | ||
| * | ||
| * @return The vector of segment lengths | ||
| */ | ||
| virtual navtypes::Vectord<N> getSegLens() const = 0; | ||
|
|
||
| /** | ||
| * @brief Check if the given joint configuration is valid. | ||
| * | ||
| * @param jointPos The joint positions to check. | ||
| * @return true iff the joint configuration is valid, false otherwise. | ||
| */ | ||
| virtual bool satisfiesConstraints(const navtypes::Vectord<N>& jointPos) const = 0; | ||
|
|
||
| /** | ||
| * @brief Get the jacobian matrix for the arm at the given joint angles. | ||
| * | ||
| * @param jointPos The joint angles of the arm. | ||
| * @return navtypes::Matrixd<D, N> The jacobian matrix at the point. | ||
| * @see https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant | ||
| */ | ||
| virtual navtypes::Matrixd<D, N> | ||
| getJacobian(const navtypes::Vectord<N>& jointPos) const = 0; | ||
|
|
||
| /** | ||
| * @brief Given a joint position, calculate the current EE position. | ||
| * | ||
| * @param jointPos Current joint position. | ||
| * @return navtypes::Vectord<D> The EE position. | ||
| */ | ||
| virtual navtypes::Vectord<D> | ||
| jointPosToEEPos(const navtypes::Vectord<N>& jointPos) const = 0; | ||
|
|
||
| /** | ||
| * @brief Calculate the EE velocity. | ||
| * | ||
| * @param jointPos Current joint position. | ||
| * @param jointVel Current velocity of each joint angle. | ||
| * @return navtypes::Vectord<D> The EE velocity. | ||
| */ | ||
| navtypes::Vectord<D> jointVelToEEVel(const navtypes::Vectord<N>& jointPos, | ||
| const navtypes::Vectord<N>& jointVel) const { | ||
| return getJacobian(jointPos) * jointVel; | ||
| } | ||
|
|
||
| /** | ||
| * @brief Calculates the joint velocity that yields the desired EE velocity. | ||
| * | ||
| * @param jointPos The current joint angles. | ||
| * @param eeVel The desired EE velocity. | ||
| * @return navtypes::Vectord<N> The joint velocities that yields the desired EE velocity. | ||
| */ | ||
| navtypes::Vectord<N> eeVelToJointVel(const navtypes::Vectord<N>& jointPos, | ||
| const Eigen::Vector2d& eeVel) const { | ||
| navtypes::Matrixd<2, N> jacobian = getJacobian(jointPos); | ||
| return jacobian.colPivHouseholderQR().solve(eeVel); | ||
| } | ||
| }; | ||
|
|
||
| } // namespace kinematics |
Oops, something went wrong.