96 lines
2.8 KiB
C++
Executable File
96 lines
2.8 KiB
C++
Executable File
#include "Scene_JointSpaceControl.h"
|
|
#include <iostream>
|
|
|
|
Scene_JointSpaceControl::Scene_JointSpaceControl(Arm& arm):
|
|
arm{arm},
|
|
deadzones({
|
|
AxisAlignedBoundingBox(
|
|
Vector3d(-125, -125, -20),
|
|
Vector3d(65, 100, 120)
|
|
),
|
|
AxisAlignedBoundingBox(
|
|
Vector3d(0, -160, 0),
|
|
Vector3d(20000, 100, 20000)
|
|
)
|
|
}),
|
|
leftVKB(L" VKBsim Gladiator EVO L "),
|
|
plot(-400, 400, 20, 2)
|
|
{
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
void Scene_JointSpaceControl::update(double frametime)
|
|
{
|
|
constexpr double speed = 5000;
|
|
VkbInputs vkbi = leftVKB.get();
|
|
|
|
// Calculate new joint positions
|
|
j0Pos += -vkbi.roll * speed * frametime;
|
|
j1Pos += -vkbi.pitch * speed * frametime;
|
|
j2Pos += -vkbi.yaw * speed * frametime;
|
|
j3Pos = vkbi.wheel * (arm.getJoints()[3].getMaxAngle() - arm.getJoints()[3].getMinAngle());
|
|
j4Pos += vkbi.thumb_ver * speed * frametime;
|
|
j5Pos += vkbi.thumb_hor * speed * frametime;
|
|
|
|
// Stage the new pose and keep it if it is good
|
|
arm.stagePose({ j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos });
|
|
if (arm.checkJointCollisions(deadzones)) {
|
|
// Roll back individual joints if their angles are out of range
|
|
arm.rollbackJointsBasedOnValidRange();
|
|
|
|
//std::cout << "Pose good!!" << std::endl;
|
|
arm.commitPose();
|
|
|
|
j0Pos = arm.getJoints()[0].getTargetPosition();
|
|
j1Pos = arm.getJoints()[1].getTargetPosition();
|
|
j2Pos = arm.getJoints()[2].getTargetPosition();
|
|
j3Pos = arm.getJoints()[3].getTargetPosition();
|
|
j4Pos = arm.getJoints()[4].getTargetPosition();
|
|
j5Pos = arm.getJoints()[5].getTargetPosition();
|
|
}
|
|
else {
|
|
arm.rollbackPose();
|
|
//std::cout << "Hit deadzone!!" << std::endl;
|
|
}
|
|
|
|
const Vector3d ef = arm.getEndEffectorJoint().getCurrentGlobalEndpoint();
|
|
//std::cout << ef << std::endl;
|
|
//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);
|
|
}
|