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>
|
||||
<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) {
|
||||
// Locally save the request data (joint angles)
|
||||
float theta1 = request->joint_angles.theta1;
|
||||
float theta2 = request->joint_angles.theta2;
|
||||
float theta3 = request->joint_angles.theta3;
|
||||
float x = 0.0;
|
||||
float y = 0.0;
|
||||
float z = 0.0;
|
||||
|
||||
```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]
|
||||
|
||||
float t = (this->SB - this->SP) * tan30 / 2;
|
||||
float y1 = -(t + this->AL * cos(theta1));
|
||||
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;
|
||||
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;
|
||||
float theta3 = request->joint_angles.theta3;
|
||||
float x = 0.0;
|
||||
float y = 0.0;
|
||||
float z = 0.0;
|
||||
|
||||
// x = (a1*z + b1)/dnm
|
||||
float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
|
||||
float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
|
||||
float t = (this->SB - this->SP) * tan30 / 2;
|
||||
float y1 = -(t + this->AL * cos(theta1));
|
||||
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;
|
||||
float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
|
||||
float b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
|
||||
// x = (a1*z + b1)/dnm
|
||||
float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
|
||||
float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
|
||||
|
||||
// a*z^2 + b*z + c = 0
|
||||
float a = a1 * a1 + a2 * a2 + dnm * dnm;
|
||||
float b = 2 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
|
||||
float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - this->PL * this->PL);
|
||||
// y = (a2*z + b2)/dnm;
|
||||
float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
|
||||
float b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
|
||||
|
||||
// discriminant
|
||||
float d = b * b - (float)4.0 * a * c;
|
||||
if (d < 0) {
|
||||
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;
|
||||
}
|
||||
// a*z^2 + b*z + c = 0
|
||||
float a = a1 * a1 + a2 * a2 + dnm * dnm;
|
||||
float b = 2 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
|
||||
float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - this->PL * this->PL);
|
||||
|
||||
// Update the response data (end effector position)
|
||||
response->x = x; // [mm]
|
||||
response->y = y; // [mm]
|
||||
response->z = z; // [mm]
|
||||
}
|
||||
// discriminant
|
||||
float d = b * b - (float)4.0 * a * c;
|
||||
if (d < 0) {
|
||||
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) {
|
||||
float y1 = -0.5 * tan30 * 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);
|
||||
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;
|
||||
}
|
||||
// Update the response data (end effector position)
|
||||
response->x = x; // [mm]
|
||||
response->y = y; // [mm]
|
||||
response->z = z; // [mm]
|
||||
}
|
||||
|
||||
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]
|
||||
float z = request->z; // [mm]
|
||||
float theta1 = 0.0;
|
||||
float theta2 = 0.0;
|
||||
float theta3 = 0.0;
|
||||
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);
|
||||
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;
|
||||
}
|
||||
|
||||
int status = this->deltaFK_AngleYZ(x, y, z, theta1);
|
||||
if (status == 0) {
|
||||
status = this->deltaFK_AngleYZ(x * cos120 + y * sin120, y * cos120 - x * sin120, z, theta2); // rotate coords to +120 deg
|
||||
} 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);
|
||||
}
|
||||
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]
|
||||
float z = request->z; // [mm]
|
||||
float theta1 = 0.0;
|
||||
float theta2 = 0.0;
|
||||
float theta3 = 0.0;
|
||||
|
||||
// Update the response data (joint angles)
|
||||
response->joint_angles.theta1 = theta1; // [rad]
|
||||
response->joint_angles.theta2 = theta2; // [rad]
|
||||
response->joint_angles.theta3 = theta3; // [rad]
|
||||
}
|
||||
```
|
||||
int status = this->deltaFK_AngleYZ(x, y, z, theta1);
|
||||
if (status == 0) {
|
||||
status = this->deltaFK_AngleYZ(x * cos120 + y * sin120, y * cos120 - x * sin120, z, theta2); // rotate coords to +120 deg
|
||||
} 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>
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue