Skip to content

Commit 51cba59

Browse files
Nikos-dNikos-d
authored
Better and fixed examples (#62)
* rename demo_ for non tutorial examples * moved print_bspline to extras/ - cleanup in manipulators * Name changes
1 parent 9523bd0 commit 51cba59

6 files changed

Lines changed: 236 additions & 103 deletions

File tree

‎blast/blast_manipulator.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -266,6 +266,7 @@ struct Manipulator {
266266

267267
} // namespace blast
268268

269-
#include "manipulator/Gen3.hpp"
269+
#include "manipulator/Kinova_Gen3.hpp"
270+
#include "manipulator/Kinova_Link6.hpp"
270271
#include "manipulator/UR5e.hpp"
271272
#include "manipulator/manipulator.hpp"

blast/manipulator/Gen3.hpp renamed to blast/manipulator/Kinova_Gen3.hpp

Lines changed: 28 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -3,34 +3,29 @@
33

44
namespace blast {
55

6-
Manipulator make_kinova_gen3() {
6+
inline Manipulator make_Kinova_Gen3() {
77
// Manipulator
88
u32 joints = 7;
99

1010
// limits
1111
ManipulatorLimits limits;
12-
limits.position_max = {INF_REAL, 2.25, INF_REAL, 2.58f, INF_REAL, 2.1f, INF_REAL}; // rad
13-
limits.position_min = -limits.position_max;
14-
15-
limits.velocity_max = {1.745f, 1.745f, 1.745f, 1.745f, 2.443f, 2.443f, 2.443f}; // rad/s
16-
// limits.vmin = -limits.velocity_max;
17-
12+
limits.position_max = {INF_REAL, 2.25f, INF_REAL, 2.58f, INF_REAL, 2.1f, INF_REAL}; // rad
13+
limits.position_min = -limits.position_max; // rad
14+
limits.velocity_max = {1.745f, 1.745f, 1.745f, 1.745f, 2.443f, 2.443f, 2.443f}; // rad/s
1815
limits.acceleration_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad/s^2
19-
// limits.amin = -limits.acceleration_max;
20-
21-
limits.torque_max = {52, 52, 52, 52, 17, 17, 17}; // Nm
22-
// limits.tau_min = -limits.torque_max;
16+
limits.torque_max = {52.0f, 52.0f, 52.0f, 52.0f, 17.0f, 17.0f, 17.0f}; // Nm
17+
limits.tool_speed_max = 1.0f;
2318

2419
// kinematic properties
2520
ManipulatorKinematics kinematics; // using default Q_base
2621
kinematics.joint_offsets = {
27-
Vec3{0.0, 0.0054, -0.1284},
28-
{0.0, -0.2104, -0.0064},
29-
{0.0, -0.0064, -0.2104},
30-
{0.0, -0.2084, -0.0064},
31-
{0.0, 0.0, -0.1059},
32-
{0.0, -0.1059, 0.0},
33-
{0.0, 0.0, -0.0615}}; // vector to next joint
22+
Vec3{0, 0.0054f, -0.1284f},
23+
{0, -0.2104f, -0.0064f},
24+
{0, -0.0064f, -0.2104f},
25+
{0, -0.2084f, -0.0064f},
26+
{0, 0, -0.1059f},
27+
{0, -0.1059f, 0},
28+
{0, 0, -0.0615f}}; // vector to next joint
3429
kinematics.joint_axes = {
3530
Vec3{0, 0, 1},
3631
{0, 0, 1},
@@ -52,7 +47,6 @@ Manipulator make_kinova_gen3() {
5247

5348
// dynamic properties
5449
ManipulatorDynamics dynamics;
55-
5650
dynamics.link_masses = {
5751
1.377f,
5852
1.1636f,
@@ -79,13 +73,13 @@ Manipulator make_kinova_gen3() {
7973
{-0.000093f, 0.000132f, -0.022905f}}; // centers of mass
8074

8175
// create manipulator gen3
82-
Manipulator generic_manip(joints, limits, kinematics, &dynamics);
76+
Manipulator gen3(joints, limits, kinematics, &dynamics);
8377

8478
// Collision model
8579
ManipulatorCapsules collisions;
8680
Sphere sphere;
87-
sphere.center = {0, 0, 0.1214};
88-
sphere.radius = 0.14;
81+
sphere.center = {0, 0, 0.1214f};
82+
sphere.radius = 0.14f;
8983
collisions.base_sphere = sphere;
9084
collisions.collision_base = {0, 0, 1};
9185

@@ -97,27 +91,28 @@ Manipulator make_kinova_gen3() {
9791

9892
// Capsule 1
9993
model_caps.joint_frame = 1;
100-
model_caps.p1 = {0, 0.035, 0};
101-
model_caps.p2 = {0, -0.425, 0};
102-
model_caps.radius = 0.06;
94+
model_caps.p1 = {0, 0.035f, 0};
95+
model_caps.p2 = {0, -0.425f, 0};
96+
model_caps.radius = 0.06f;
10397
collisions.capsule_list.push_back(model_caps);
10498

10599
// Capsule 2
106100
model_caps.joint_frame = 3;
107-
model_caps.p1 = {0, 0, -0.025};
108-
model_caps.p2 = {0, -0.3, -0.01};
109-
model_caps.radius = 0.06;
101+
model_caps.p1 = {0, 0, -0.025f};
102+
model_caps.p2 = {0, -0.3f, -0.01f};
103+
model_caps.radius = 0.06f;
110104
collisions.capsule_list.push_back(model_caps);
111105

112106
// Capsule 3
113107
model_caps.joint_frame = 5;
114-
model_caps.p1 = {0, 0, -0.015};
115-
model_caps.p2 = {0.0, -0.15, -0.015};
116-
model_caps.radius = 0.055;
108+
model_caps.p1 = {0, 0, -0.015f};
109+
model_caps.p2 = {0, -0.15f, -0.015f};
110+
model_caps.radius = 0.055f;
117111
collisions.capsule_list.push_back(model_caps);
118112

119-
generic_manip.set_capsules(collisions);
113+
gen3.set_capsules(collisions);
120114

121-
return generic_manip;
115+
return gen3;
122116
}
117+
123118
} // namespace blast

‎blast/manipulator/Kinova_Link6.hpp‎

Lines changed: 134 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
#pragma once
2+
#include <blast>
3+
4+
namespace blast {
5+
6+
inline Manipulator make_Kinova_Link6() {
7+
// Manipulator
8+
u32 joints = 6;
9+
10+
// limits
11+
ManipulatorLimits limits;
12+
limits.position_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad
13+
limits.position_min = -limits.position_max; // rad
14+
limits.velocity_max = {3.4907f, 3.4907f, 3.4907f, 5.5851f, 5.5851f, 5.5851f}; // rad/s
15+
// 10.472 equals to the value of 600 deg/s^2 in rad/s^2
16+
limits.acceleration_max = {10.472f, 10.472f, 10.472f, 10.472f, 10.472f, 10.472f}; // rad/s^2
17+
limits.torque_max = {210.0f, 210.0f, 210.0f, 100.0f, 100.0f, 100.0f}; // Nm
18+
limits.tool_speed_max = 1.0f; // m/s
19+
20+
// kinematic properties
21+
ManipulatorKinematics kinematics; // using default Q_base
22+
kinematics.joint_offsets = {
23+
Vec3{0.11024f, -0.06926f, -0.1375f},
24+
{0, 0.4850f, 0},
25+
{0, -0.15216f, -0.0917f},
26+
{0, -0.06296f, -0.22275f},
27+
{0.08703f, 0.0860f, -0.07692f},
28+
{0, 0, -0.0920f}}; // vector to next joint
29+
kinematics.joint_axes = {
30+
Vec3{0, 0, 1},
31+
{0, 0, 1},
32+
{0, 0, 1},
33+
{0, 0, 1},
34+
{0, 0, 1},
35+
{0, 0, 1}}; // direction vectors of joint
36+
kinematics.first_joint_position = {0.0, 0.0, 0.0530f};
37+
kinematics.base_position = {0.0, 0.0, 0.0};
38+
kinematics.base_rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1};
39+
kinematics.static_rotations[0] = {1, 0, 0, 0, -1, 0, 0, 0, -1};
40+
kinematics.static_rotations[1] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
41+
kinematics.static_rotations[2] = {1, 0, 0, 0, -1, 0, 0, 0, -1};
42+
kinematics.static_rotations[3] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
43+
kinematics.static_rotations[4] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
44+
kinematics.static_rotations[5] = {0, 0, 1, 0, 1, 0, -1, 0, 0};
45+
kinematics.static_rotations[6] = {1, 0, 0, 0, -1, 0, 0, 0, -1}; // rotation matrix to tool
46+
47+
// dynamic properties
48+
ManipulatorDynamics dynamics;
49+
dynamics.link_masses = {
50+
4.8257f,
51+
5.9860f,
52+
3.4159f,
53+
2.0849f,
54+
2.0076f,
55+
1.5193f}; // link masses
56+
dynamics.inertia_tensors = {
57+
Mat3{0.0192746f, -0.00239802f, -0.00896331f, -0.00239802f, 0.03087806f, 0.0016298f, -0.00896331f, 0.0016298f, 0.02134949f},
58+
{0.25899206f, -2.89E-05f, -1.23E-06f, -2.89E-05f, 0.01755445f, -0.02128064f, -1.23E-06f, -0.02128064f, 0.25291674f},
59+
{0.01742043f, -3.55E-06f, 8.4E-07f, -3.55E-06f, 0.01119175f, 0.00518163f, 8.4E-07f, 0.00518163f, 0.01212876f},
60+
{0.02454276f, 2.61E-06f, 1.799E-05f, 2.61E-06f, 0.02385702f, 0.00315758f, 1.799E-05f, 0.00315758f, 0.00294903f},
61+
{0.00734684f, 0.00124927f, -0.00090156f, 0.00124927f, 0.00464684f, -0.00236128f, -0.00090156f, -0.00236128f, 0.00589508f},
62+
{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
63+
dynamics.cog_offsets = {
64+
Vec3{0.03930119f, -0.00705889f, -0.08462154f},
65+
{2.53E-06f, 0.18829586f, -0.03988382f},
66+
{4.64E-06f, -0.02451414f, -0.02997969f},
67+
{-0.00010793f, -0.01056422f, -0.08091102f},
68+
{0.01243595f, 0.03284165f, -0.04091434f},
69+
{0.0f, 0.00050624f, -0.00388589f}}; // centers of mass
70+
71+
// Collision model
72+
ManipulatorCapsules collisions;
73+
Sphere base;
74+
base.center = {0, 0, 0.0}; // because this is relative to p_base and p_base is {0, 0, 0.053}
75+
base.radius = 0.2375f;
76+
collisions.base_sphere = base;
77+
collisions.collision_base = {0, 0, 0, 1, 1, 1};
78+
79+
collisions.collision_matrix.resize(6, 6);
80+
collisions.collision_matrix(5, 0) = 1;
81+
collisions.collision_matrix(4, 1) = 1;
82+
collisions.collision_matrix(5, 1) = 1;
83+
84+
// Collision model
85+
CollisionModelCapsule model_caps;
86+
87+
// Capsule 1
88+
model_caps.joint_frame = 1;
89+
model_caps.p1 = {0, 0, -0.065f};
90+
model_caps.p2 = {0, 0, 0.045f};
91+
model_caps.radius = 0.065f;
92+
collisions.capsule_list.push_back(model_caps);
93+
94+
// Capsule 2
95+
model_caps.joint_frame = 1;
96+
model_caps.p1 = {0, 0, -0.08f};
97+
model_caps.p2 = {0, 0.485f, -0.08f};
98+
model_caps.radius = 0.065f;
99+
collisions.capsule_list.push_back(model_caps);
100+
101+
// Capsule 3
102+
model_caps.joint_frame = 2;
103+
model_caps.p1 = {0, 0, -0.065f};
104+
model_caps.p2 = {0, 0, 0.085f};
105+
model_caps.radius = 0.065f;
106+
collisions.capsule_list.push_back(model_caps);
107+
108+
// Capsule 4
109+
model_caps.joint_frame = 2;
110+
model_caps.p1 = {0, 0.00695f, -0.0917f};
111+
model_caps.p2 = {0, -0.36805f, -0.0917f};
112+
model_caps.radius = 0.061f;
113+
collisions.capsule_list.push_back(model_caps);
114+
115+
// Capsule 5
116+
model_caps.joint_frame = 4;
117+
model_caps.p1 = {0, 0, 0};
118+
model_caps.p2 = {0, 0, -0.08f};
119+
model_caps.radius = 0.060f;
120+
collisions.capsule_list.push_back(model_caps);
121+
122+
// Capsule 6
123+
model_caps.joint_frame = 5;
124+
model_caps.p1 = {0, 0, 0.08583f};
125+
model_caps.p2 = {0, 0, -0.06417f};
126+
model_caps.radius = 0.060f;
127+
collisions.capsule_list.push_back(model_caps);
128+
129+
Manipulator link6(joints, limits, kinematics, &dynamics, &collisions);
130+
131+
return link6;
132+
}
133+
134+
} // namespace blast

0 commit comments

Comments
 (0)