49 lines
No EOL
1.8 KiB
C++
49 lines
No EOL
1.8 KiB
C++
// --------------------------------------------------------------------------------------
|
|
// Project: MicroManipulatorStepper
|
|
// License: MIT (see LICENSE file for full description)
|
|
// All text in here must be included in any redistribution.
|
|
// Author: M. S. (diffraction limited)
|
|
// --------------------------------------------------------------------------------------
|
|
|
|
#pragma once
|
|
|
|
//*** INCLUDE ***************************************************************************
|
|
|
|
#include "kinematic_model_base.h"
|
|
#include "utilities/math3d.h"
|
|
|
|
//*** CLASS *****************************************************************************
|
|
|
|
class Pose6DF;
|
|
|
|
//--- KinemtaicModel_Delta3D ------------------------------------------------------------
|
|
|
|
class KinematicModel_Delta3D : public IKinemtaicModel {
|
|
public:
|
|
KinematicModel_Delta3D();
|
|
|
|
int get_joint_count();
|
|
bool foreward(const float* joint_positions, Pose6DF& pose) override;
|
|
bool inverse(const Pose6DF& pose, float* joint_positions) override;
|
|
|
|
void test();
|
|
|
|
protected:
|
|
Vec3F arm_attachment_point(int joint_idx, float rotor_angle);
|
|
|
|
public:
|
|
float arm_length;
|
|
float rotor_radius;
|
|
float rotor_angle_offset[3]; // angle offset of the neutral position from zero position in rad
|
|
Pose6DF base_to_actuator[3];
|
|
Pose6DF actuator_to_base[3];
|
|
Vec3F ee_attachment_points[3]; // EE arm attachment points in endeffector coordinate system
|
|
};
|
|
|
|
//*** FUNCTION **************************************************************************
|
|
|
|
bool circle_sphere_intersection(double r1, const Vec3F& p, double r2, Vec3F intersections[2]);
|
|
bool three_sphere_intersection(const Vec3F& p1, float r1,
|
|
const Vec3F& p2, float r2,
|
|
const Vec3F& p3, float r3,
|
|
Vec3F intersections[2]); |