Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
pinocchio
Commits
f18f5ca2
Verified
Commit
f18f5ca2
authored
Apr 28, 2021
by
Justin Carpentier
Browse files
algo/models: fix the case where the list of joints is empty
parent
deb9ed3b
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/algorithm/model.hxx
View file @
f18f5ca2
...
...
@@ -300,7 +300,7 @@ namespace pinocchio
typedef
typename
Model
::
JointData
JointData
;
typedef
typename
Model
::
Frame
Frame
;
typedef
typename
Model
::
SE3
SE3
;
// Sort indexes
std
::
sort
(
list_of_joints_to_lock
.
begin
(),
list_of_joints_to_lock
.
end
());
...
...
@@ -317,7 +317,6 @@ namespace pinocchio
reduced_model
.
joints
.
reserve
((
size_t
)
njoints
);
reduced_model
.
jointPlacements
.
reserve
((
size_t
)
njoints
);
reduced_model
.
parents
.
reserve
((
size_t
)
njoints
);
reduced_model
.
inertias
.
reserve
((
size_t
)
njoints
);
reduced_model
.
names
[
0
]
=
input_model
.
names
[
0
];
reduced_model
.
joints
[
0
]
=
input_model
.
joints
[
0
];
...
...
@@ -369,16 +368,12 @@ namespace pinocchio
Frame
frame
=
Frame
(
joint_name
,
reduced_parent_joint_index
,
reduced_previous_frame_index
,
liMi
,
FIXED_JOINT
);
FIXED_JOINT
,
input_model
.
inertias
[
joint_id
]);
FrameIndex
frame_id
=
reduced_model
.
addFrame
(
frame
);
reduced_model
.
frames
[
frame_id
].
previousFrame
=
frame_id
;
// a bit weird, but this is a solution for missing parent frame
// Add the Inertia of the link supported by joint_id
reduced_model
.
appendBodyToJoint
(
reduced_parent_joint_index
,
input_model
.
inertias
[
joint_id
],
frame
.
placement
);
current_index_to_lock
++
;
}
else
...
...
@@ -436,34 +431,34 @@ namespace pinocchio
for
(
++
frame_it
;
frame_it
!=
input_model
.
frames
.
end
();
++
frame_it
)
{
const
Frame
&
input_frame
=
*
frame_it
;
const
std
::
string
&
inpu
t_joint_name
=
input_model
.
names
[
input_frame
.
parent
];
const
std
::
string
&
suppor
t_joint_name
=
input_model
.
names
[
input_frame
.
parent
];
std
::
vector
<
JointIndex
>::
const_iterator
joint_
id_
it
=
std
::
find
(
list_of_joints_to_lock
.
begin
(),
list_of_joints_to_lock
.
end
(),
input_frame
.
parent
);
std
::
vector
<
JointIndex
>::
const_iterator
support_
joint_it
=
std
::
find
(
list_of_joints_to_lock
.
begin
(),
list_of_joints_to_lock
.
end
(),
input_frame
.
parent
);
if
(
joint_
id_
it
!=
list_of_joints_to_lock
.
end
())
if
(
support_
joint_it
!=
list_of_joints_to_lock
.
end
())
{
if
(
input_frame
.
type
==
JOINT
&&
reduced_model
.
existFrame
(
input_frame
.
name
)
&&
inpu
t_joint_name
==
input_frame
.
name
)
&&
suppor
t_joint_name
==
input_frame
.
name
)
continue
;
// this means that the Joint is now fixed and has been replaced by a Frame. No need to add a new one.
// The joint has been removed and replaced by a Frame
const
FrameIndex
joint_frame_id
=
reduced_model
.
getFrameId
(
inpu
t_joint_name
);
const
FrameIndex
joint_frame_id
=
reduced_model
.
getFrameId
(
suppor
t_joint_name
);
const
Frame
&
joint_frame
=
reduced_model
.
frames
[
joint_frame_id
];
Frame
reduced_frame
=
input_frame
;
reduced_frame
.
placement
=
joint_frame
.
placement
*
input_frame
.
placement
;
reduced_frame
.
parent
=
joint_frame
.
parent
;
reduced_frame
.
previousFrame
=
reduced_model
.
getFrameId
(
input_model
.
frames
[
input_frame
.
previousFrame
].
name
);
reduced_model
.
addFrame
(
reduced_frame
);
reduced_model
.
addFrame
(
reduced_frame
,
false
);
}
else
{
Frame
reduced_frame
=
input_frame
;
reduced_frame
.
parent
=
reduced_model
.
getJointId
(
input_model
.
names
[
input_frame
.
parent
]);
reduced_frame
.
previousFrame
=
reduced_model
.
getFrameId
(
input_model
.
frames
[
input_frame
.
previousFrame
].
name
);
reduced_model
.
addFrame
(
reduced_frame
);
reduced_model
.
addFrame
(
reduced_frame
,
false
);
}
}
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment