diff --git a/blast/blast_manipulator.hpp b/blast/blast_manipulator.hpp index 608f461..de29033 100644 --- a/blast/blast_manipulator.hpp +++ b/blast/blast_manipulator.hpp @@ -266,6 +266,7 @@ struct Manipulator { } // namespace blast -#include "manipulator/Gen3.hpp" +#include "manipulator/Kinova_Gen3.hpp" +#include "manipulator/Kinova_Link6.hpp" #include "manipulator/UR5e.hpp" #include "manipulator/manipulator.hpp" diff --git a/blast/manipulator/Gen3.hpp b/blast/manipulator/Kinova_Gen3.hpp similarity index 66% rename from blast/manipulator/Gen3.hpp rename to blast/manipulator/Kinova_Gen3.hpp index cfc3ae3..7883619 100644 --- a/blast/manipulator/Gen3.hpp +++ b/blast/manipulator/Kinova_Gen3.hpp @@ -3,34 +3,29 @@ namespace blast { -Manipulator make_kinova_gen3() { +inline Manipulator make_Kinova_Gen3() { // Manipulator u32 joints = 7; // limits ManipulatorLimits limits; - limits.position_max = {INF_REAL, 2.25, INF_REAL, 2.58f, INF_REAL, 2.1f, INF_REAL}; // rad - limits.position_min = -limits.position_max; - - limits.velocity_max = {1.745f, 1.745f, 1.745f, 1.745f, 2.443f, 2.443f, 2.443f}; // rad/s - // limits.vmin = -limits.velocity_max; - + limits.position_max = {INF_REAL, 2.25f, INF_REAL, 2.58f, INF_REAL, 2.1f, INF_REAL}; // rad + limits.position_min = -limits.position_max; // rad + limits.velocity_max = {1.745f, 1.745f, 1.745f, 1.745f, 2.443f, 2.443f, 2.443f}; // rad/s limits.acceleration_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad/s^2 - // limits.amin = -limits.acceleration_max; - - limits.torque_max = {52, 52, 52, 52, 17, 17, 17}; // Nm - // limits.tau_min = -limits.torque_max; + limits.torque_max = {52.0f, 52.0f, 52.0f, 52.0f, 17.0f, 17.0f, 17.0f}; // Nm + limits.tool_speed_max = 1.0f; // kinematic properties ManipulatorKinematics kinematics; // using default Q_base kinematics.joint_offsets = { - Vec3{0.0, 0.0054, -0.1284}, - {0.0, -0.2104, -0.0064}, - {0.0, -0.0064, -0.2104}, - {0.0, -0.2084, -0.0064}, - {0.0, 0.0, -0.1059}, - {0.0, -0.1059, 0.0}, - {0.0, 0.0, -0.0615}}; // vector to next joint + Vec3{0, 0.0054f, -0.1284f}, + {0, -0.2104f, -0.0064f}, + {0, -0.0064f, -0.2104f}, + {0, -0.2084f, -0.0064f}, + {0, 0, -0.1059f}, + {0, -0.1059f, 0}, + {0, 0, -0.0615f}}; // vector to next joint kinematics.joint_axes = { Vec3{0, 0, 1}, {0, 0, 1}, @@ -52,7 +47,6 @@ Manipulator make_kinova_gen3() { // dynamic properties ManipulatorDynamics dynamics; - dynamics.link_masses = { 1.377f, 1.1636f, @@ -79,13 +73,13 @@ Manipulator make_kinova_gen3() { {-0.000093f, 0.000132f, -0.022905f}}; // centers of mass // create manipulator gen3 - Manipulator generic_manip(joints, limits, kinematics, &dynamics); + Manipulator gen3(joints, limits, kinematics, &dynamics); // Collision model ManipulatorCapsules collisions; Sphere sphere; - sphere.center = {0, 0, 0.1214}; - sphere.radius = 0.14; + sphere.center = {0, 0, 0.1214f}; + sphere.radius = 0.14f; collisions.base_sphere = sphere; collisions.collision_base = {0, 0, 1}; @@ -97,27 +91,28 @@ Manipulator make_kinova_gen3() { // Capsule 1 model_caps.joint_frame = 1; - model_caps.p1 = {0, 0.035, 0}; - model_caps.p2 = {0, -0.425, 0}; - model_caps.radius = 0.06; + model_caps.p1 = {0, 0.035f, 0}; + model_caps.p2 = {0, -0.425f, 0}; + model_caps.radius = 0.06f; collisions.capsule_list.push_back(model_caps); // Capsule 2 model_caps.joint_frame = 3; - model_caps.p1 = {0, 0, -0.025}; - model_caps.p2 = {0, -0.3, -0.01}; - model_caps.radius = 0.06; + model_caps.p1 = {0, 0, -0.025f}; + model_caps.p2 = {0, -0.3f, -0.01f}; + model_caps.radius = 0.06f; collisions.capsule_list.push_back(model_caps); // Capsule 3 model_caps.joint_frame = 5; - model_caps.p1 = {0, 0, -0.015}; - model_caps.p2 = {0.0, -0.15, -0.015}; - model_caps.radius = 0.055; + model_caps.p1 = {0, 0, -0.015f}; + model_caps.p2 = {0, -0.15f, -0.015f}; + model_caps.radius = 0.055f; collisions.capsule_list.push_back(model_caps); - generic_manip.set_capsules(collisions); + gen3.set_capsules(collisions); - return generic_manip; + return gen3; } + } // namespace blast diff --git a/blast/manipulator/Kinova_Link6.hpp b/blast/manipulator/Kinova_Link6.hpp new file mode 100644 index 0000000..b9d68b1 --- /dev/null +++ b/blast/manipulator/Kinova_Link6.hpp @@ -0,0 +1,134 @@ +#pragma once +#include + +namespace blast { + +inline Manipulator make_Kinova_Link6() { + // Manipulator + u32 joints = 6; + + // limits + ManipulatorLimits limits; + limits.position_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad + limits.position_min = -limits.position_max; // rad + limits.velocity_max = {3.4907f, 3.4907f, 3.4907f, 5.5851f, 5.5851f, 5.5851f}; // rad/s + // 10.472 equals to the value of 600 deg/s^2 in rad/s^2 + limits.acceleration_max = {10.472f, 10.472f, 10.472f, 10.472f, 10.472f, 10.472f}; // rad/s^2 + limits.torque_max = {210.0f, 210.0f, 210.0f, 100.0f, 100.0f, 100.0f}; // Nm + limits.tool_speed_max = 1.0f; // m/s + + // kinematic properties + ManipulatorKinematics kinematics; // using default Q_base + kinematics.joint_offsets = { + Vec3{0.11024f, -0.06926f, -0.1375f}, + {0, 0.4850f, 0}, + {0, -0.15216f, -0.0917f}, + {0, -0.06296f, -0.22275f}, + {0.08703f, 0.0860f, -0.07692f}, + {0, 0, -0.0920f}}; // vector to next joint + kinematics.joint_axes = { + Vec3{0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}}; // direction vectors of joint + kinematics.first_joint_position = {0.0, 0.0, 0.0530f}; + kinematics.base_position = {0.0, 0.0, 0.0}; + kinematics.base_rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + kinematics.static_rotations[0] = {1, 0, 0, 0, -1, 0, 0, 0, -1}; + kinematics.static_rotations[1] = {1, 0, 0, 0, 0, -1, 0, 1, 0}; + kinematics.static_rotations[2] = {1, 0, 0, 0, -1, 0, 0, 0, -1}; + kinematics.static_rotations[3] = {1, 0, 0, 0, 0, -1, 0, 1, 0}; + kinematics.static_rotations[4] = {1, 0, 0, 0, 0, -1, 0, 1, 0}; + kinematics.static_rotations[5] = {0, 0, 1, 0, 1, 0, -1, 0, 0}; + kinematics.static_rotations[6] = {1, 0, 0, 0, -1, 0, 0, 0, -1}; // rotation matrix to tool + + // dynamic properties + ManipulatorDynamics dynamics; + dynamics.link_masses = { + 4.8257f, + 5.9860f, + 3.4159f, + 2.0849f, + 2.0076f, + 1.5193f}; // link masses + dynamics.inertia_tensors = { + Mat3{0.0192746f, -0.00239802f, -0.00896331f, -0.00239802f, 0.03087806f, 0.0016298f, -0.00896331f, 0.0016298f, 0.02134949f}, + {0.25899206f, -2.89E-05f, -1.23E-06f, -2.89E-05f, 0.01755445f, -0.02128064f, -1.23E-06f, -0.02128064f, 0.25291674f}, + {0.01742043f, -3.55E-06f, 8.4E-07f, -3.55E-06f, 0.01119175f, 0.00518163f, 8.4E-07f, 0.00518163f, 0.01212876f}, + {0.02454276f, 2.61E-06f, 1.799E-05f, 2.61E-06f, 0.02385702f, 0.00315758f, 1.799E-05f, 0.00315758f, 0.00294903f}, + {0.00734684f, 0.00124927f, -0.00090156f, 0.00124927f, 0.00464684f, -0.00236128f, -0.00090156f, -0.00236128f, 0.00589508f}, + {0.00390762f, -1.13E-06f, 1.16E-06f, -1.13E-06f, 0.00390722f, -2.21E-05f, 1.16E-06f, -2.21E-05f, 0.0013928f}}; // Inertial tensors + dynamics.cog_offsets = { + Vec3{0.03930119f, -0.00705889f, -0.08462154f}, + {2.53E-06f, 0.18829586f, -0.03988382f}, + {4.64E-06f, -0.02451414f, -0.02997969f}, + {-0.00010793f, -0.01056422f, -0.08091102f}, + {0.01243595f, 0.03284165f, -0.04091434f}, + {0.0f, 0.00050624f, -0.00388589f}}; // centers of mass + + // Collision model + ManipulatorCapsules collisions; + Sphere base; + base.center = {0, 0, 0.0}; // because this is relative to p_base and p_base is {0, 0, 0.053} + base.radius = 0.2375f; + collisions.base_sphere = base; + collisions.collision_base = {0, 0, 0, 1, 1, 1}; + + collisions.collision_matrix.resize(6, 6); + collisions.collision_matrix(5, 0) = 1; + collisions.collision_matrix(4, 1) = 1; + collisions.collision_matrix(5, 1) = 1; + + // Collision model + CollisionModelCapsule model_caps; + + // Capsule 1 + model_caps.joint_frame = 1; + model_caps.p1 = {0, 0, -0.065f}; + model_caps.p2 = {0, 0, 0.045f}; + model_caps.radius = 0.065f; + collisions.capsule_list.push_back(model_caps); + + // Capsule 2 + model_caps.joint_frame = 1; + model_caps.p1 = {0, 0, -0.08f}; + model_caps.p2 = {0, 0.485f, -0.08f}; + model_caps.radius = 0.065f; + collisions.capsule_list.push_back(model_caps); + + // Capsule 3 + model_caps.joint_frame = 2; + model_caps.p1 = {0, 0, -0.065f}; + model_caps.p2 = {0, 0, 0.085f}; + model_caps.radius = 0.065f; + collisions.capsule_list.push_back(model_caps); + + // Capsule 4 + model_caps.joint_frame = 2; + model_caps.p1 = {0, 0.00695f, -0.0917f}; + model_caps.p2 = {0, -0.36805f, -0.0917f}; + model_caps.radius = 0.061f; + collisions.capsule_list.push_back(model_caps); + + // Capsule 5 + model_caps.joint_frame = 4; + model_caps.p1 = {0, 0, 0}; + model_caps.p2 = {0, 0, -0.08f}; + model_caps.radius = 0.060f; + collisions.capsule_list.push_back(model_caps); + + // Capsule 6 + model_caps.joint_frame = 5; + model_caps.p1 = {0, 0, 0.08583f}; + model_caps.p2 = {0, 0, -0.06417f}; + model_caps.radius = 0.060f; + collisions.capsule_list.push_back(model_caps); + + Manipulator link6(joints, limits, kinematics, &dynamics, &collisions); + + return link6; +} + +} // namespace blast diff --git a/blast/manipulator/UR5e.hpp b/blast/manipulator/UR5e.hpp index 2fe7d15..95fb729 100644 --- a/blast/manipulator/UR5e.hpp +++ b/blast/manipulator/UR5e.hpp @@ -3,70 +3,72 @@ namespace blast { -Manipulator make_UR5e() { +inline Manipulator make_UR5e() { // Manipulator - blast::u32 joints = 6; + u32 joints = 6; // limits - blast::ManipulatorLimits limits; - limits.position_max = {6.283200, 6.283200, 3.141600, 6.283200, 6.283200, 6.283200}; - limits.position_min = {-6.283200, -6.283200, -3.141600, -6.283200, -6.283200, -6.283200}; - limits.velocity_max = {3.141600, 3.141600, 3.141600, 3.141600, 3.141600, 3.141600}; - // limits.vmin = {-3.141600, -3.141600, -3.141600, -3.141600, -3.141600, -3.141600}; - limits.acceleration_max = {13.96, 13.96, 13.96, 13.96, 13.96, 13.96}; - // limits.amin = {-13.96, -13.96, -13.96, -13.96, -13.96, -13.96}; - limits.torque_max = {150.000000, 150.000000, 150.000000, 28.000000, 28.000000, 28.000000}; - // limits.tau_min = {-150.000000, -150.000000, -150.000000, -28.000000, -28.000000, -28.000000}; - limits.tool_speed_max = 2.0; // todo: verify this value - - blast::ManipulatorKinematics kinematics; + ManipulatorLimits limits; + limits.position_max = {6.283200f, 6.283200f, 3.141600f, 6.283200f, 6.283200f, 6.283200f}; // rad + limits.position_min = {-6.283200f, -6.283200f, -3.141600f, -6.283200f, -6.283200f, -6.283200f}; // rad + limits.velocity_max = {3.141600f, 3.141600f, 3.141600f, 3.141600f, 3.141600f, 3.141600f}; // rad/s + limits.acceleration_max = {13.96f, 13.96f, 13.96f, 13.96f, 13.96f, 13.96f}; // rad/s^2 + limits.torque_max = {150.0f, 150.0f, 150.0f, 28.0f, 28.0f, 28.0f}; // Nm + limits.tool_speed_max = 1.0f; // m/s + + // kinematic properties + ManipulatorKinematics kinematics; // using default Q_base kinematics.joint_offsets = { Vec3{0, 0, 0}, - {-0.425, 0, 0}, - {-0.3922, 0, 0.1333}, - {0, -0.0997, -0}, - {0, 0.0996, -0}, - {0, 0, 0} // to end effector - }; + {-0.425f, 0, 0}, + {-0.3922f, 0, 0.1333f}, + {0, -0.0997f, -0}, + {0, 0.0996f, -0}, + {0, 0, 0}}; // vector to next joint kinematics.joint_axes = { Vec3{0, 0, 1}, {0, 0, 1}, {0, 0, 1}, {0, 0, 1}, {0, 0, 1}, - {0, 0, 1}}; - // kinematics.static_rotations.resize(6); - kinematics.static_rotations[0] = {-1.000000, 0.000000, 0.000000, -0.000000, -1.000000, 0.000000, 0.000000, -0.000000, 1.000000}; // modified - kinematics.static_rotations[1] = {1.000000, 0.000000, 0.000000, -0.000000, -0.000000, 1.000000, 0.000000, -1.000000, -0.000000}; - kinematics.static_rotations[2] = {1.000000, 0.000000, 0.000000, -0.000000, 1.000000, 0.000000, 0.000000, -0.000000, 1.000000}; - kinematics.static_rotations[3] = {1.000000, 0.000000, 0.000000, -0.000000, 1.000000, 0.000000, 0.000000, -0.000000, 1.000000}; - kinematics.static_rotations[4] = {1.000000, 0.000000, 0.000000, -0.000000, -0.000000, 1.000000, 0.000000, -1.000000, -0.000000}; - kinematics.static_rotations[5] = {1.000000, -0.000000, 0.000000, 0.000000, -0.000000, -1.000000, 0.000000, 1.000000, -0.000000}; - // kinematics.static_rotations[6] = {1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000}; - kinematics.first_joint_position = {0.000000, 0.000000, 0.162500}; - - blast::ManipulatorDynamics dynamics; - dynamics.link_masses = {3.700000, 8.393000, 2.275000, 1.219000, 1.219000, 0.187900}; + {0, 0, 1}}; // direction vectors of joint + kinematics.static_rotations[0] = {-1, 0, 0, -0, -1, 0, 0, -0, 1}; + kinematics.static_rotations[1] = {1, 0, 0, -0, -0, 1, 0, -1, -0}; + kinematics.static_rotations[2] = {1, 0, 0, -0, 1, 0, 0, -0, 1}; + kinematics.static_rotations[3] = {1, 0, 0, -0, 1, 0, 0, -0, 1}; + kinematics.static_rotations[4] = {1, 0, 0, -0, -0, 1, 0, -1, -0}; + kinematics.static_rotations[5] = {1, -0, 0, 0, -0, -1, 0, 1, -0}; + kinematics.first_joint_position = {0, 0, 0.162500f}; + + // dynamic properties + ManipulatorDynamics dynamics; + dynamics.link_masses = { + 3.700000f, + 8.393000f, + 2.275000f, + 1.219000f, + 1.219000f, + 0.187900f}; // link masses dynamics.inertia_tensors = { - Mat3{0.0103, 0, 0, 0, 0.0103, 0, 0, 0, 0.0067}, - {0.1339, 0, 0, 0, 0.1339, 0, 0, 0, 0.0151}, - {0.0312, 0, 0, 0, 0.0312, 0, 0, 0, 0.0041}, - {0.0026, 0, 0, 0, 0.0026, 0, 0, 0, 0.0022}, - {0.0026, 0, 0, 0, 0.0026, 0, 0, 0, 0.0022}, - {0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001}}; + Mat3{0.0103f, 0, 0, 0, 0.0103f, 0, 0, 0, 0.0067f}, + {0.1339f, 0, 0, 0, 0.1339f, 0, 0, 0, 0.0151f}, + {0.0312f, 0, 0, 0, 0.0312f, 0, 0, 0, 0.0041f}, + {0.0026f, 0, 0, 0, 0.0026f, 0, 0, 0, 0.0022f}, + {0.0026f, 0, 0, 0, 0.0026f, 0, 0, 0, 0.0022f}, + {0.0001f, 0, 0, 0, 0.0001f, 0, 0, 0, 0.0001f}}; // Inertial tensors dynamics.cog_offsets = { Vec3{0, 0, 0}, - {-0.2125, 0, 0.138}, - {-0.1961, 0, 0.007}, + {-0.2125f, 0, 0.138f}, + {-0.1961f, 0, 0.007f}, {0, 0, 0}, {0, 0, 0}, - {0, 0, -0.0229}}; + {0, 0, -0.0229f}}; // centers of mass - blast::ManipulatorCapsules collisions; + ManipulatorCapsules collisions; - blast::Sphere base; - base.center = {0, 0, 0.0325}; - base.radius = 0.09; + Sphere base; + base.center = {0, 0, 0.0325f}; + base.radius = 0.09f; collisions.base_sphere = base; collisions.collision_base = {0, 0, 0, 1, 1, 1, 1}; @@ -94,51 +96,51 @@ Manipulator make_UR5e() { collisions.collision_matrix(3, 6) = 1; - blast::CollisionModelCapsule capsule; + CollisionModelCapsule capsule; capsule.joint_frame = 0; capsule.p1 = {0, 0, 0}; - capsule.p2 = {0, -0.15, 0}; - capsule.radius = 0.09; + capsule.p2 = {0, -0.15f, 0}; + capsule.radius = 0.09f; collisions.capsule_list.push_back(capsule); capsule.joint_frame = 1; - capsule.p1 = {-0.42, 0, 0.1375}; - capsule.p2 = {0, 0, 0.1375}; - capsule.radius = 0.06; + capsule.p1 = {-0.42f, 0, 0.1375f}; + capsule.p2 = {0, 0, 0.1375f}; + capsule.radius = 0.06f; collisions.capsule_list.push_back(capsule); capsule.joint_frame = 2; - capsule.p1 = {0, 0, 0.02}; - capsule.p2 = {0, 0, 0.18}; - capsule.radius = 0.065; + capsule.p1 = {0, 0, 0.02f}; + capsule.p2 = {0, 0, 0.18f}; + capsule.radius = 0.065f; collisions.capsule_list.push_back(capsule); capsule.joint_frame = 2; - capsule.p1 = {-0.373156, 0, 0.00850418}; - capsule.p2 = {0.000440611, 0.000121443, 0.00850418}; - capsule.radius = 0.05; + capsule.p1 = {-0.373156f, 0, 0.00850418f}; + capsule.p2 = {0.000440611f, 0.000121443f, 0.00850418f}; + capsule.radius = 0.05f; collisions.capsule_list.push_back(capsule); capsule.joint_frame = 3; capsule.p1 = {0, 0, 0}; - capsule.p2 = {0, 0, -0.155}; - capsule.radius = 0.0425; + capsule.p2 = {0, 0, -0.155f}; + capsule.radius = 0.0425f; collisions.capsule_list.push_back(capsule); capsule.joint_frame = 3; - capsule.p1 = {0, 0.03, 0}; - capsule.p2 = {0, -0.1, 0}; - capsule.radius = 0.0425; + capsule.p1 = {0, 0.03f, 0}; + capsule.p2 = {0, -0.1f, 0}; + capsule.radius = 0.0425f; collisions.capsule_list.push_back(capsule); capsule.joint_frame = 5; - capsule.p1 = {0, 0, -0.14}; - capsule.p2 = {0, 0, -0.01}; - capsule.radius = 0.038; + capsule.p1 = {0, 0, -0.14f}; + capsule.p2 = {0, 0, -0.01f}; + capsule.radius = 0.038f; collisions.capsule_list.push_back(capsule); - blast::Manipulator generic_manip(joints, limits, kinematics, &dynamics, &collisions); - return generic_manip; + Manipulator ur5e(joints, limits, kinematics, &dynamics, &collisions); + return ur5e; } } // namespace blast diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 96e1885..0472b94 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -16,7 +16,8 @@ set(EXAMPLES example_01_forward_kinematics example_01_forward_kinematics.cpp example_02_trajectory_optimization example_02_trajectory_optimization.cpp example_03_collision_avoidance example_03_collision_avoidance.cpp - get_bspline_and_basis_functions get_bspline_and_basis_functions.cpp + + print_bsplines_and_basis_functions extras/print_bsplines_and_basis_functions.cpp ICRA2026_benchmark_speedups ICRA2026/ICRA2026_benchmark_speedups.cpp ) diff --git a/examples/get_bspline_and_basis_functions.cpp b/examples/extras/print_bsplines_and_basis_functions.cpp similarity index 100% rename from examples/get_bspline_and_basis_functions.cpp rename to examples/extras/print_bsplines_and_basis_functions.cpp