formatting
This commit is contained in:
Sharwin24
parent
eba2bdbbc6
commit
524ed7ec09
1 changed files with 92 additions and 85 deletions
|
|
@ -29,101 +29,108 @@ The forward and inverse kinematics were then implemented in C++ following the ap
|
||||||
<div>
|
<div>
|
||||||
<details>
|
<details>
|
||||||
<summary>Forward and Inverse Kinematics C++ Implementation</summary>
|
<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
|
||||||
// Locally save the request data (joint angles)
|
// this->AL = Active Link Length [mm]
|
||||||
float theta1 = request->joint_angles.theta1;
|
// this->PL = Passive Link Length [mm]
|
||||||
float theta2 = request->joint_angles.theta2;
|
// this->SB = Base Side Length [mm]
|
||||||
float theta3 = request->joint_angles.theta3;
|
// this->SP = Platform (EE) Side Length [mm]
|
||||||
float x = 0.0;
|
|
||||||
float y = 0.0;
|
|
||||||
float z = 0.0;
|
|
||||||
|
|
||||||
float t = (this->SB - this->SP) * tan30 / 2;
|
void DeltaKinematics::forwardKinematics(const std::shared_ptr<DeltaFK::Request> request, std::shared_ptr<DeltaFK::Response> response) {
|
||||||
float y1 = -(t + this->AL * cos(theta1));
|
// Locally save the request data (joint angles)
|
||||||
float z1 = -this->AL * sin(theta1);
|
float theta1 = request->joint_angles.theta1;
|
||||||
float y2 = (t + this->AL * cos(theta2)) * sin30;
|
float theta2 = request->joint_angles.theta2;
|
||||||
float x2 = y2 * tan60;
|
float theta3 = request->joint_angles.theta3;
|
||||||
float z2 = -this->AL * sin(theta2);
|
float x = 0.0;
|
||||||
float y3 = (t + this->AL * cos(theta3)) * sin30;
|
float y = 0.0;
|
||||||
float x3 = -y3 * tan60;
|
float z = 0.0;
|
||||||
float z3 = -this->AL * sin(theta3);
|
|
||||||
float dnm = (y2 - y1) * x3 - (y3 - y1) * x2;
|
|
||||||
float w1 = y1 * y1 + z1 * z1;
|
|
||||||
float w2 = x2 * x2 + y2 * y2 + z2 * z2;
|
|
||||||
float w3 = x3 * x3 + y3 * y3 + z3 * z3;
|
|
||||||
|
|
||||||
// x = (a1*z + b1)/dnm
|
float t = (this->SB - this->SP) * tan30 / 2;
|
||||||
float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
|
float y1 = -(t + this->AL * cos(theta1));
|
||||||
float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
|
float z1 = -this->AL * sin(theta1);
|
||||||
|
float y2 = (t + this->AL * cos(theta2)) * sin30;
|
||||||
|
float x2 = y2 * tan60;
|
||||||
|
float z2 = -this->AL * sin(theta2);
|
||||||
|
float y3 = (t + this->AL * cos(theta3)) * sin30;
|
||||||
|
float x3 = -y3 * tan60;
|
||||||
|
float z3 = -this->AL * sin(theta3);
|
||||||
|
float dnm = (y2 - y1) * x3 - (y3 - y1) * x2;
|
||||||
|
float w1 = y1 * y1 + z1 * z1;
|
||||||
|
float w2 = x2 * x2 + y2 * y2 + z2 * z2;
|
||||||
|
float w3 = x3 * x3 + y3 * y3 + z3 * z3;
|
||||||
|
|
||||||
// y = (a2*z + b2)/dnm;
|
// x = (a1*z + b1)/dnm
|
||||||
float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
|
float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
|
||||||
float b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
|
float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
|
||||||
|
|
||||||
// a*z^2 + b*z + c = 0
|
// y = (a2*z + b2)/dnm;
|
||||||
float a = a1 * a1 + a2 * a2 + dnm * dnm;
|
float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
|
||||||
float b = 2 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
|
float b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
|
||||||
float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - this->PL * this->PL);
|
|
||||||
|
|
||||||
// discriminant
|
// a*z^2 + b*z + c = 0
|
||||||
float d = b * b - (float)4.0 * a * c;
|
float a = a1 * a1 + a2 * a2 + dnm * dnm;
|
||||||
if (d < 0) {
|
float b = 2 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
|
||||||
RCLCPP_ERROR(this->get_logger(), "DeltaFK: Invalid Configuration (%f, %f, %f) [rad]", theta1, theta2, theta3);
|
float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - this->PL * this->PL);
|
||||||
} else {
|
|
||||||
z = (-b + sqrt(d)) / (2*a);
|
|
||||||
x = (a1 * z + b1) / dnm;
|
|
||||||
y = (a2 * z + b2) / dnm;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Update the response data (end effector position)
|
// discriminant
|
||||||
response->x = x; // [mm]
|
float d = b * b - (float)4.0 * a * c;
|
||||||
response->y = y; // [mm]
|
if (d < 0) {
|
||||||
response->z = z; // [mm]
|
RCLCPP_ERROR(this->get_logger(), "DeltaFK: Invalid Configuration (%f, %f, %f) [rad]", theta1, theta2, theta3);
|
||||||
}
|
} else {
|
||||||
|
z = (-b + sqrt(d)) / (2*a);
|
||||||
|
x = (a1 * z + b1) / dnm;
|
||||||
|
y = (a2 * z + b2) / dnm;
|
||||||
|
}
|
||||||
|
|
||||||
int DeltaKinematics::deltaFK_AngleYZ(float x0, float y0, float z0, float& theta) {
|
// Update the response data (end effector position)
|
||||||
float y1 = -0.5 * tan30 * SB; // Half base * tan(30)
|
response->x = x; // [mm]
|
||||||
y0 -= 0.5 * tan30 * this->SP; // shift center to edge
|
response->y = y; // [mm]
|
||||||
// z = a + b*y
|
response->z = z; // [mm]
|
||||||
float a = (x0 * x0 + y0 * y0 + z0 * z0 + this->AL * this->AL - this->PL * this->PL - y1 * y1) / (2 * z0);
|
}
|
||||||
float b = (y1 - y0) / z0;
|
|
||||||
// discriminant
|
|
||||||
float d = -(a + b * y1) * (a + b * y1) + this->AL * (b * b * this->AL + this->AL);
|
|
||||||
if (d < 0) return -1; // non-existing point
|
|
||||||
float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
|
|
||||||
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) {
|
int DeltaKinematics::deltaFK_AngleYZ(float x0, float y0, float z0, float& theta) {
|
||||||
// Locally save the request data (end effector position)
|
float y1 = -0.5 * tan30 * this->SB; // Half base * tan(30)
|
||||||
float x = request->x; // [mm]
|
y0 -= 0.5 * tan30 * this->SP; // shift center to edge
|
||||||
float y = request->y; // [mm]
|
// z = a + b*y
|
||||||
float z = request->z; // [mm]
|
float a = (x0 * x0 + y0 * y0 + z0 * z0 + this->AL * this->AL - this->PL * this->PL - y1 * y1) / (2 * z0);
|
||||||
float theta1 = 0.0;
|
float b = (y1 - y0) / z0;
|
||||||
float theta2 = 0.0;
|
// discriminant
|
||||||
float theta3 = 0.0;
|
float d = -(a + b * y1) * (a + b * y1) + this->AL * (b * b * this->AL + this->AL);
|
||||||
|
if (d < 0) return -1; // non-existing point
|
||||||
|
float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
|
||||||
|
float zj = a + b * yj;
|
||||||
|
theta = atan(-zj / (y1 - yj)) + ((yj > y1) ? M_PI : 0.0);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
int status = this->deltaFK_AngleYZ(x, y, z, theta1);
|
void DeltaKinematics::inverseKinematics(const std::shared_ptr<DeltaIK::Request> request, std::shared_ptr<DeltaIK::Response> response) {
|
||||||
if (status == 0) {
|
// Locally save the request data (end effector position)
|
||||||
status = this->deltaFK_AngleYZ(x * cos120 + y * sin120, y * cos120 - x * sin120, z, theta2); // rotate coords to +120 deg
|
float x = request->x; // [mm]
|
||||||
} else {
|
float y = request->y; // [mm]
|
||||||
RCLCPP_ERROR(this->get_logger(), "DeltaIK: Non-existing point (%f, %f, %f) [mm]", x, y, z);
|
float z = request->z; // [mm]
|
||||||
}
|
float theta1 = 0.0;
|
||||||
if (status == 0) {
|
float theta2 = 0.0;
|
||||||
status = this->deltaFK_AngleYZ(x * cos120 - y * sin120, y * cos120 + x * sin120, z, theta3); // rotate coords to -120 deg
|
float theta3 = 0.0;
|
||||||
} else {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "DeltaIK: Non-existing point (%f, %f, %f) [mm]", x, y, z);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Update the response data (joint angles)
|
int status = this->deltaFK_AngleYZ(x, y, z, theta1);
|
||||||
response->joint_angles.theta1 = theta1; // [rad]
|
if (status == 0) {
|
||||||
response->joint_angles.theta2 = theta2; // [rad]
|
status = this->deltaFK_AngleYZ(x * cos120 + y * sin120, y * cos120 - x * sin120, z, theta2); // rotate coords to +120 deg
|
||||||
response->joint_angles.theta3 = theta3; // [rad]
|
} else {
|
||||||
}
|
RCLCPP_ERROR(this->get_logger(), "DeltaIK: Non-existing point (%f, %f, %f) [mm]", x, y, z);
|
||||||
```
|
}
|
||||||
|
if (status == 0) {
|
||||||
|
status = this->deltaFK_AngleYZ(x * cos120 - y * sin120, y * cos120 + x * sin120, z, theta3); // rotate coords to -120 deg
|
||||||
|
} else {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "DeltaIK: Non-existing point (%f, %f, %f) [mm]", x, y, z);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update the response data (joint angles)
|
||||||
|
response->joint_angles.theta1 = theta1; // [rad]
|
||||||
|
response->joint_angles.theta2 = theta2; // [rad]
|
||||||
|
response->joint_angles.theta3 = theta3; // [rad]
|
||||||
|
}
|
||||||
|
```
|
||||||
|
</details>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue