Skip to content

Nikos/fix examples #62

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
gallantandre merged 3 commits into from
May 31, 2026
Merged

Nikos/fix examples #62

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
56b1d4e
rename demo_ for non tutorial examples
Nikos-d May 30, 2026
bab268f
moved print_bspline to extras/
Nikos-d May 31, 2026
cc1575b
Name changes
Nikos-d May 31, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion blast/blast_manipulator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
61 changes: 28 additions & 33 deletions blast/manipulator/Gen3.hpp → blast/manipulator/Kinova_Gen3.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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},
Expand All @@ -52,7 +47,6 @@ Manipulator make_kinova_gen3() {

// dynamic properties
ManipulatorDynamics dynamics;

dynamics.link_masses = {
1.377f,
1.1636f,
Expand All @@ -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};

Expand All @@ -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
134 changes: 134 additions & 0 deletions blast/manipulator/Kinova_Link6.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,134 @@
#pragma once
#include <blast>

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
Loading
Loading