implement experimental ccdik
This commit is contained in:
Leonetienne
@@ -6,6 +6,7 @@
|
||||
#include "StateMachine.h"
|
||||
#include "ArmSegment.h"
|
||||
#include "Scene_CCDIK.h"
|
||||
#include "Scene_JointSpaceControl.h"
|
||||
|
||||
MainLoop::MainLoop() noexcept :
|
||||
arm(L"COM4")
|
||||
@@ -33,7 +34,8 @@ void MainLoop::run()
|
||||
static std::chrono::steady_clock::time_point lastBegin =
|
||||
std::chrono::high_resolution_clock::now();
|
||||
|
||||
Scene_CCDIK sceneCCDIK(arm);
|
||||
Scene_CCDIK scene(arm);
|
||||
//Scene_JointSpaceControl scene(arm);
|
||||
|
||||
while (isRunning)
|
||||
{
|
||||
@@ -52,7 +54,7 @@ void MainLoop::run()
|
||||
case StateMachine::STATE::IDLING:
|
||||
case StateMachine::STATE::MOVING: {
|
||||
// HERE SCENE UPDATE
|
||||
sceneCCDIK.update(frametime);
|
||||
scene.update(frametime);
|
||||
|
||||
// Unset MOVING to IDLING state if no arm is moving
|
||||
if (StateMachine::getInstance().getState() == StateMachine::STATE::MOVING && !arm.isMoving()) {
|
||||
|
||||
+176
-13
@@ -1,5 +1,9 @@
|
||||
#include "Scene_CCDIK.h"
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
|
||||
#undef min
|
||||
#undef max
|
||||
|
||||
Scene_CCDIK::Scene_CCDIK(Arm& arm) :
|
||||
arm{ arm },
|
||||
@@ -14,14 +18,8 @@ Scene_CCDIK::Scene_CCDIK(Arm& arm) :
|
||||
)
|
||||
}),
|
||||
leftVKB(L" VKBsim Gladiator EVO L "),
|
||||
cursor3d(0,0,0)
|
||||
cursor3d(0, 300, 0)
|
||||
{
|
||||
|
||||
if (!arm.assumeHomePoseImmediate()) {
|
||||
std::cerr << "Unable to move arm to home!" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
if (!leftVKB.connect()) {
|
||||
std::cerr << "Unable to connect to left VKB controller!" << std::endl;
|
||||
exit(-1);
|
||||
@@ -32,20 +30,185 @@ void Scene_CCDIK::update(double frametime)
|
||||
{
|
||||
vkbi = leftVKB.get();
|
||||
|
||||
constexpr double speed = 5000;
|
||||
VkbInputs vkbi = leftVKB.get();
|
||||
update3dCursorSquare(frametime);
|
||||
//update3dCursorByVKBI(frametime);
|
||||
performCcdIK(cursor3d);
|
||||
|
||||
update3dCursor(frametime);
|
||||
const double error = (cursor3d - arm.getEndEffectorJoint().getTargetedGlobalEndpoint()).magnitude();
|
||||
//std::cout
|
||||
// << std::fixed
|
||||
// << std::setprecision(1)
|
||||
// << std::noshowpos
|
||||
// << "Cursor: "
|
||||
// //<< arm.getEndEffectorJoint().getTargetedGlobalEndpoint()
|
||||
// << cursor3d
|
||||
// << std::endl;
|
||||
|
||||
arm.update(frametime);
|
||||
}
|
||||
|
||||
void Scene_CCDIK::update3dCursor(double frametime)
|
||||
void Scene_CCDIK::update3dCursorByVKBI(double frametime)
|
||||
{
|
||||
constexpr double cursorSpeed = 1.0;
|
||||
constexpr double cursorSpeed = 50000.0;
|
||||
cursor3d += Vector3d(
|
||||
vkbi.roll * cursorSpeed * frametime,
|
||||
vkbi.yaw * cursorSpeed * frametime,
|
||||
-vkbi.pitch * cursorSpeed * frametime
|
||||
vkbi.pitch * cursorSpeed * frametime
|
||||
);
|
||||
}
|
||||
|
||||
void Scene_CCDIK::update3dCursorSquare(double frametime)
|
||||
{
|
||||
static double t = 0.0;
|
||||
t += frametime;
|
||||
|
||||
const double centerX = 0.0;
|
||||
const double centerY = 200.0;
|
||||
const double zPlane = 100.0;
|
||||
|
||||
const double halfWidth = 150.0; // X range: [-150, +150]
|
||||
const double halfHeight = 100.0; // Y range: [100, 300] SAFE
|
||||
|
||||
const double speed = 20.0;
|
||||
|
||||
double phase = std::fmod(t * speed, 1.0);
|
||||
|
||||
if (phase < 0.25)
|
||||
{
|
||||
double u = phase / 0.25;
|
||||
cursor3d = Vector3d(
|
||||
centerX - halfWidth + 2.0 * halfWidth * u,
|
||||
centerY - halfHeight,
|
||||
zPlane
|
||||
);
|
||||
}
|
||||
else if (phase < 0.50)
|
||||
{
|
||||
double u = (phase - 0.25) / 0.25;
|
||||
cursor3d = Vector3d(
|
||||
centerX + halfWidth,
|
||||
centerY - halfHeight + 2.0 * halfHeight * u,
|
||||
zPlane
|
||||
);
|
||||
}
|
||||
else if (phase < 0.75)
|
||||
{
|
||||
double u = (phase - 0.50) / 0.25;
|
||||
cursor3d = Vector3d(
|
||||
centerX + halfWidth - 2.0 * halfWidth * u,
|
||||
centerY + halfHeight,
|
||||
zPlane
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
double u = (phase - 0.75) / 0.25;
|
||||
cursor3d = Vector3d(
|
||||
centerX - halfWidth,
|
||||
centerY + halfHeight - 2.0 * halfHeight * u,
|
||||
zPlane
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static inline double clampd(double v, double lo, double hi)
|
||||
{
|
||||
return std::max(lo, std::min(v, hi));
|
||||
}
|
||||
|
||||
// Proof of concept, this method not my code
|
||||
void Scene_CCDIK::performCcdIK(const Vector3d& target)
|
||||
{
|
||||
const int MAX_ITERS = 25;
|
||||
const double POS_EPS = 0.002;
|
||||
const double ANG_EPS = 1e-4;
|
||||
const double MAX_STEP = 0.35;
|
||||
const double GAIN = 0.85;
|
||||
|
||||
auto& joints = arm.getJoints();
|
||||
|
||||
auto jointOrigin = [&](std::size_t i) -> Vector3d
|
||||
{
|
||||
const ArmSegment* p = joints[i].getParent();
|
||||
return p ? p->getTargetedGlobalEndpoint()
|
||||
: Vector3d{ 0.0, 0.0, 0.0 };
|
||||
};
|
||||
|
||||
auto endEffector = [&]() -> Vector3d
|
||||
{
|
||||
return joints.back().getTargetedGlobalEndpoint();
|
||||
};
|
||||
|
||||
for (int iter = 0; iter < MAX_ITERS; ++iter)
|
||||
{
|
||||
if ((target - endEffector()).magnitude() <= POS_EPS)
|
||||
break;
|
||||
|
||||
for (int ji = static_cast<int>(NUM_JOINTS) - 1; ji >= 0; --ji)
|
||||
{
|
||||
ArmSegment& j = joints[static_cast<std::size_t>(ji)];
|
||||
|
||||
const Vector3d origin = jointOrigin(static_cast<std::size_t>(ji));
|
||||
const Vector3d eePos = endEffector();
|
||||
|
||||
Vector3d r1 = eePos - origin;
|
||||
Vector3d r2 = target - origin;
|
||||
|
||||
double r1l = r1.magnitude();
|
||||
double r2l = r2.magnitude();
|
||||
if (r1l < 1e-9 || r2l < 1e-9)
|
||||
continue;
|
||||
|
||||
r1 /= r1l;
|
||||
r2 /= r2l;
|
||||
|
||||
const double theta0 = j.getTargetPosition();
|
||||
|
||||
j.stageAngle(theta0 + ANG_EPS);
|
||||
const Vector3d eePlus = endEffector();
|
||||
j.stageAngle(theta0);
|
||||
|
||||
Vector3d dEe = (eePlus - eePos) / ANG_EPS;
|
||||
|
||||
Vector3d rVec = eePos - origin;
|
||||
Vector3d axis = rVec.cross(dEe);
|
||||
double axisLen = axis.magnitude();
|
||||
if (axisLen < 1e-12)
|
||||
continue;
|
||||
axis /= axisLen;
|
||||
|
||||
Vector3d p1 = r1 - axis * r1.dot(axis);
|
||||
Vector3d p2 = r2 - axis * r2.dot(axis);
|
||||
|
||||
double p1l = p1.magnitude();
|
||||
double p2l = p2.magnitude();
|
||||
if (p1l < 1e-9 || p2l < 1e-9)
|
||||
continue;
|
||||
|
||||
p1 /= p1l;
|
||||
p2 /= p2l;
|
||||
|
||||
double c = clampd(p1.dot(p2), -1.0, 1.0);
|
||||
double s = axis.dot(p1.cross(p2));
|
||||
double dTheta = std::atan2(s, c);
|
||||
|
||||
dTheta *= GAIN;
|
||||
dTheta = clampd(dTheta, -MAX_STEP, MAX_STEP);
|
||||
|
||||
double newTheta = theta0 + dTheta;
|
||||
newTheta = clampd(newTheta, j.getMinAngle(), j.getMaxAngle());
|
||||
|
||||
j.stageAngle(newTheta);
|
||||
|
||||
arm.rollbackJointsBasedOnValidRange();
|
||||
if (!arm.checkJointCollisions(deadzones))
|
||||
arm.rollbackPose();
|
||||
|
||||
if ((target - endEffector()).magnitude() <= POS_EPS)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
arm.commitPose();
|
||||
}
|
||||
|
||||
@@ -12,7 +12,9 @@ public:
|
||||
void update(double frametime);
|
||||
|
||||
private:
|
||||
void update3dCursor(double frametime);
|
||||
void update3dCursorByVKBI(double frametime);
|
||||
void update3dCursorSquare(double frametime);
|
||||
void performCcdIK(const Vector3d& target);
|
||||
|
||||
Arm& arm;
|
||||
std::array<AxisAlignedBoundingBox, 2> deadzones;
|
||||
|
||||
@@ -14,7 +14,7 @@ Scene_JointSpaceControl::Scene_JointSpaceControl(Arm& arm):
|
||||
)
|
||||
}),
|
||||
leftVKB(L" VKBsim Gladiator EVO L "),
|
||||
plot(-200, 200, 20, 2)
|
||||
plot(-400, 400, 20, 2)
|
||||
{
|
||||
|
||||
if (!arm.assumeHomePoseImmediate()) {
|
||||
@@ -67,5 +67,29 @@ void Scene_JointSpaceControl::update(double frametime)
|
||||
//std::cout << arm.getJ3().getCurrentPosition() << std::endl;
|
||||
plot.draw(ef);
|
||||
|
||||
//std::cout
|
||||
// << std::fixed
|
||||
// << std::setprecision(1)
|
||||
// << std::noshowpos
|
||||
// << "j0: "
|
||||
// << j0Pos
|
||||
// << std::endl
|
||||
// << "j1: "
|
||||
// << j1Pos
|
||||
// << std::endl
|
||||
// << "j2: "
|
||||
// << j2Pos
|
||||
// << std::endl
|
||||
// << "j3: "
|
||||
// << j3Pos
|
||||
// << std::endl
|
||||
// << "j4: "
|
||||
// << j4Pos
|
||||
// << std::endl
|
||||
// << "j5: "
|
||||
// << j5Pos
|
||||
// << std::endl;
|
||||
//system("cls");
|
||||
|
||||
arm.update(frametime);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user