Verified Commit 11f17f36 authored by Gabriele Buondonno's avatar Gabriele Buondonno Committed by Justin Carpentier
Browse files

[all] [minor] Remove warnings and useless conversions between int and FrameIndex

parent e4530499
......@@ -364,9 +364,8 @@ namespace pinocchio
liMi,
FIXED_JOINT);
int frame_id = reduced_model.addFrame(frame);
assert(frame_id >= 0);
reduced_model.frames[(size_t)frame_id].previousFrame = (FrameIndex)frame_id; // a bit weird, but this a solution for missing parent frame
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,
......
......@@ -205,11 +205,9 @@ namespace pinocchio
{
const Frame & frame = model.frames[parentFrameId];
int fid = model.addFrame(Frame(joint_name, frame.parent, parentFrameId,
FrameIndex fid = model.addFrame(Frame(joint_name, frame.parent, parentFrameId,
frame.placement * joint_placement, FIXED_JOINT)
);
if (fid < 0)
throw std::invalid_argument("Fixed joint " + joint_name + " could not be added.");
appendBodyToJoint((FrameIndex)fid, Y, SE3::Identity(), body_name);
}
......
......@@ -87,8 +87,7 @@ BOOST_AUTO_TEST_CASE ( test_update_placements )
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
pinocchio::Data data(model);
pinocchio::Data data_ref(model);
......@@ -113,8 +112,7 @@ BOOST_AUTO_TEST_CASE ( test_update_single_placement )
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
pinocchio::Data data(model);
pinocchio::Data data_ref(model);
......@@ -139,8 +137,7 @@ BOOST_AUTO_TEST_CASE ( test_velocity )
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
pinocchio::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
......@@ -173,8 +170,7 @@ BOOST_AUTO_TEST_CASE ( test_acceleration )
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
pinocchio::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
......@@ -208,8 +204,7 @@ BOOST_AUTO_TEST_CASE ( test_classic_acceleration )
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
pinocchio::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
......@@ -268,7 +263,7 @@ BOOST_AUTO_TEST_CASE(test_frame_getters)
// Build a simple 1R planar model
Model model;
JointIndex parentId = model.addJoint(0, JointModelRZ(), SE3::Identity(), "Joint1");
FrameIndex frameId = (FrameIndex)(model.addFrame(Frame("Frame1", parentId, 0, SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), OP_FRAME)));
FrameIndex frameId = model.addFrame(Frame("Frame1", parentId, 0, SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), OP_FRAME));
Data data(model);
......
......@@ -164,8 +164,8 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
// First append a model to the universe frame.
Model model1;
GeometryModel geomModel1;
int fid = 0;
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, (FrameIndex)fid,
FrameIndex fid = 0;
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid,
SE3::Identity(), model1, geomModel1);
Data data1 (model1);
......@@ -182,9 +182,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
humanoid.getFrameId("humanoid/chest2_joint"), aMb,
OP_FRAME));
BOOST_CHECK(fid >= 0);
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, (FrameIndex)fid,
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid,
SE3::Identity(), model, geomModel);
BOOST_TEST_MESSAGE(model);
......
......@@ -99,7 +99,7 @@ BOOST_AUTO_TEST_CASE(test_frame_body_regressor)
JointIndex JOINT_ID = JointIndex(model.njoints) - 1;
const SE3 & framePlacement = SE3::Random();
FrameIndex FRAME_ID = (FrameIndex) model.addBodyFrame ("test_body", JOINT_ID, framePlacement, -1);
FrameIndex FRAME_ID = model.addBodyFrame ("test_body", JOINT_ID, framePlacement, -1);
pinocchio::Data data(model);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment