-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathrobotBuildforCodegen.m
More file actions
105 lines (88 loc) · 4.75 KB
/
Copy pathrobotBuildforCodegen.m
File metadata and controls
105 lines (88 loc) · 4.75 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
function robot = robotBuildforCodegen(dataFormat)
%robotBuildforCodegen Create rigidBodyTree for the robot model
% ROBOT = robotBuildforCodegen(DATAFORMAT) constructs a rigidBodyTree, ROBOT, and sets the
% data format to DATAFORMAT. The possible values of DATAFORMAT are
% 'struct', 'column' and 'row'. The default value is 'row', which
% matches the data format of the rigidbodytree object used to generate
% this function.
% Auto-generated by MATLAB on 10-Feb-2026 18:10:07
%#codegen
narginchk(0,1);
if ~nargin==1
dataFormat='row';
end
robot = rigidBodyTree('MaxNumBodies', 3, 'DataFormat', dataFormat);
robot.Gravity = [ 0, 0, 0];
robot.BaseName = 'base';
% Add body, 'Body1', and joint, 'jnt1'
bodyName = 'Body1';
bodyMass = 1;
bodyCoM = [ 0, 0, 0];
bodyInertia = [ 1, 1, 1, 0, 0, 0];
parentName = 'base';
jointName = 'jnt1';
jointType = 'revolute';
T_Joint_to_Parent = [ 1, 0, 0, 0; ...
0, 1, 0, 0; ...
0, 0, 1, 0; ...
0, 0, 0, 1];
joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [ 0, 0, 1];
jointPositionLimits = [ -3.141592653589793, 3.141592653589793];
jointHomePosition = 0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;
body = rigidBody(bodyName, "MaxNumCollisions", 0);
body.Joint = joint;
body.Mass = bodyMass;
body.CenterOfMass = bodyCoM;
body.Inertia = bodyInertia;
robot.addBody(body, parentName);
% Add body, 'body2', and joint, 'jnt2'
bodyName = 'body2';
bodyMass = 1;
bodyCoM = [ 0, 0, 0];
bodyInertia = [ 1, 1, 1, 0, 0, 0];
parentName = 'Body1';
jointName = 'jnt2';
jointType = 'revolute';
T_Joint_to_Parent = [ 1, 0, 0, 0.3; ...
0, 1, 0, 0; ...
0, 0, 1, 0; ...
0, 0, 0, 1];
joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [ 0, 0, 1];
jointPositionLimits = [ -3.141592653589793, 3.141592653589793];
jointHomePosition = 0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;
body = rigidBody(bodyName, "MaxNumCollisions", 0);
body.Joint = joint;
body.Mass = bodyMass;
body.CenterOfMass = bodyCoM;
body.Inertia = bodyInertia;
robot.addBody(body, parentName);
% Add body, 'tool_tip', and joint, 'jnt3'
bodyName = 'tool_tip';
bodyMass = 1;
bodyCoM = [ 0, 0, 0];
bodyInertia = [ 1, 1, 1, 0, 0, 0];
parentName = 'body2';
jointName = 'jnt3';
jointType = 'fixed';
T_Joint_to_Parent = [ 1, 0, 0, 0.3; ...
0, 1, 0, 0; ...
0, 0, 1, 0; ...
0, 0, 0, 1];
joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
body = rigidBody(bodyName, "MaxNumCollisions", 0);
body.Joint = joint;
body.Mass = bodyMass;
body.CenterOfMass = bodyCoM;
body.Inertia = bodyInertia;
robot.addBody(body, parentName);