FunctionalHacker/hugo-toha
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

formatting

Browse source
This commit is contained in:
Sharwin24 2025-02-25 11:26:08 -06:00
parent eba2bdbbc6
commit 524ed7ec09
1 changed files with 92 additions and 85 deletions
Download patch file Download diff file Expand all files Collapse all files

25
content/posts/delta-robot/index.md
View file

@ -29,8 +29,14 @@ The forward and inverse kinematics were then implemented in C++ following the ap
<div>
<details>
<summary>Forward and Inverse Kinematics C++ Implementation</summary>
```cpp
void DeltaKinematics::forwardKinematics(const std::shared_ptr<DeltaFK::Request> request, std::shared_ptr<DeltaFK::Response> response) {
```cpp
// this->AL = Active Link Length [mm]
// this->PL = Passive Link Length [mm]
// this->SB = Base Side Length [mm]
// this->SP = Platform (EE) Side Length [mm]
void DeltaKinematics::forwardKinematics(const std::shared_ptr<DeltaFK::Request> request, std::shared_ptr<DeltaFK::Response> response) {
// Locally save the request data (joint angles)
float theta1 = request->joint_angles.theta1;
float theta2 = request->joint_angles.theta2;
@ -80,10 +86,10 @@ void DeltaKinematics::forwardKinematics(const std::shared_ptr<DeltaFK::Request>
response->x = x; // [mm]
response->y = y; // [mm]
response->z = z; // [mm]
}
}
int DeltaKinematics::deltaFK_AngleYZ(float x0, float y0, float z0, float& theta) {
float y1 = -0.5 * tan30 * SB; // Half base * tan(30)
int DeltaKinematics::deltaFK_AngleYZ(float x0, float y0, float z0, float& theta) {
float y1 = -0.5 * tan30 * this->SB; // Half base * tan(30)
y0 -= 0.5 * tan30 * this->SP; // shift center to edge
// z = a + b*y
float a = (x0 * x0 + y0 * y0 + z0 * z0 + this->AL * this->AL - this->PL * this->PL - y1 * y1) / (2 * z0);
@ -95,9 +101,9 @@ int DeltaKinematics::deltaFK_AngleYZ(float x0, float y0, float z0, float& theta)
float zj = a + b * yj;
theta = atan(-zj / (y1 - yj)) + ((yj > y1) ? M_PI : 0.0);
return 0;
}
}
void DeltaKinematics::inverseKinematics(const std::shared_ptr<DeltaIK::Request> request, std::shared_ptr<DeltaIK::Response> response) {
void DeltaKinematics::inverseKinematics(const std::shared_ptr<DeltaIK::Request> request, std::shared_ptr<DeltaIK::Response> response) {
// Locally save the request data (end effector position)
float x = request->x; // [mm]
float y = request->y; // [mm]
@ -122,8 +128,9 @@ void DeltaKinematics::inverseKinematics(const std::shared_ptr<DeltaIK::Request>
response->joint_angles.theta1 = theta1; // [rad]
response->joint_angles.theta2 = theta2; // [rad]
response->joint_angles.theta3 = theta3; // [rad]
}
```
}
```
</details>
</div>