diff --git a/content/posts/delta-robot/index.md b/content/posts/delta-robot/index.md index 4705a5a..ad46d87 100644 --- a/content/posts/delta-robot/index.md +++ b/content/posts/delta-robot/index.md @@ -29,101 +29,108 @@ The forward and inverse kinematics were then implemented in C++ following the ap
Forward and Inverse Kinematics C++ Implementation -```cpp -void DeltaKinematics::forwardKinematics(const std::shared_ptr request, std::shared_ptr 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 request, std::shared_ptr 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 request, std::shared_ptr 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 request, std::shared_ptr 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] + } + ``` +