133 lines
2.8 KiB
C++
Executable File
133 lines
2.8 KiB
C++
Executable File
#include <vector>
|
|
#include "ArmSegment.h"
|
|
#include "OSerialBus.h"
|
|
#include "Matrix3x3.h"
|
|
|
|
ArmSegment::ArmSegment(
|
|
OSerialBus& serialBus,
|
|
const Vector3d& rotationAxis,
|
|
const Vector3d& longitudinalAxis,
|
|
const double length,
|
|
const uint8_t motorIndex,
|
|
const double safeMinAngle,
|
|
const double safeMaxAngle,
|
|
const double homeAngle,
|
|
ArmSegment* parent
|
|
) noexcept :
|
|
rotationAxis(rotationAxis),
|
|
longitudinalAxis(longitudinalAxis),
|
|
length(length),
|
|
homeAngle(homeAngle),
|
|
motor(
|
|
motorIndex,
|
|
serialBus,
|
|
homeAngle,
|
|
safeMinAngle,
|
|
safeMaxAngle,
|
|
1.0
|
|
),
|
|
parent(parent)
|
|
{
|
|
|
|
}
|
|
|
|
bool ArmSegment::goHome(const std::optional<double> speed) noexcept
|
|
{
|
|
return motor.moveTo(homeAngle, speed);
|
|
}
|
|
|
|
|
|
bool ArmSegment::goHomeImmediate() noexcept
|
|
{
|
|
return motor.moveToImmediate(homeAngle);
|
|
}
|
|
|
|
bool ArmSegment::moveTo(const double theta, const std::optional<double> speed) noexcept
|
|
{
|
|
return motor.moveTo(theta, speed);
|
|
}
|
|
|
|
|
|
bool ArmSegment::moveToImmediate(const double theta) noexcept
|
|
{
|
|
return motor.moveToImmediate(theta);
|
|
}
|
|
|
|
bool ArmSegment::moveBy(const double theta, const std::optional<double> speed) noexcept
|
|
{
|
|
return motor.moveBy(theta, speed);
|
|
}
|
|
|
|
|
|
bool ArmSegment::moveByImmediate(const double theta) noexcept
|
|
{
|
|
return motor.moveByImmediate(theta);
|
|
}
|
|
|
|
bool ArmSegment::stopMoving() noexcept
|
|
{
|
|
return motor.stopMoving();
|
|
}
|
|
|
|
void ArmSegment::pauseMoving() noexcept
|
|
{
|
|
motor.pauseMoving();
|
|
}
|
|
|
|
void ArmSegment::resumeMoving() noexcept
|
|
{
|
|
motor.resumeMoving();
|
|
}
|
|
|
|
bool ArmSegment::assumeTargetPositionImmediately() noexcept
|
|
{
|
|
return motor.assumeTargetPositionImmediately();
|
|
}
|
|
|
|
void ArmSegment::update(const double frametime)
|
|
{
|
|
motor.update(frametime);
|
|
}
|
|
|
|
Vector3d ArmSegment::getGlobalEndpoint() const
|
|
{
|
|
// Collect the chain from this segment up to the root
|
|
std::vector<const ArmSegment*> chain;
|
|
chain.reserve(3);
|
|
const ArmSegment* s = this;
|
|
while (s != nullptr) {
|
|
chain.push_back(s);
|
|
s = s->getParent();
|
|
}
|
|
std::reverse(chain.begin(), chain.end()); // now from base to this segment
|
|
|
|
// Forward kinematics: accumulate rotation and position
|
|
Matrix3x3 r = Matrix3x3::identity();
|
|
Vector3d p(0.0, 0.0, 0.0);
|
|
|
|
for (const ArmSegment* seg : chain) {
|
|
r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, seg->motor.getCurrentPosition()); // LOCAL axis
|
|
p += r * (seg->longitudinalAxis * seg->length);
|
|
}
|
|
|
|
return p;
|
|
}
|
|
|
|
Vector3d ArmSegment::getGlobalAngle() const
|
|
{
|
|
Matrix3x3 r = Matrix3x3::fromEulerXYZ(getLocalAngle());
|
|
|
|
const ArmSegment* s = parent;
|
|
while (s != nullptr) {
|
|
r *= Matrix3x3::fromEulerXYZ(s->getLocalAngle());
|
|
s = s->getParent();
|
|
}
|
|
|
|
return r.toEulerXYZ();
|
|
}
|
|
|
|
Vector3d ArmSegment::getLocalAngle() const
|
|
{
|
|
return rotationAxis * motor.getCurrentPosition();
|
|
}
|