Commit 99d92f0d authored by Jordan Lack's avatar Jordan Lack

Remove a bunch of raw pointers in model

parent 34fffad9
Pipeline #55622026 canceled with stages
......@@ -112,7 +112,7 @@ namespace RobotDynamics
*
* \note To query the number of degrees of freedom use Model::dof_count.
*/
struct RDL_DLLAPI Model
struct Model
{
Model();
......@@ -122,20 +122,20 @@ struct RDL_DLLAPI Model
// Structural information
std::shared_ptr<ReferenceFrame> worldFrame; /**< Pointer to world frame */
ReferenceFramePtr worldFrame; /**< Pointer to world frame */
/* clang-format off */
std::vector<std::shared_ptr<ReferenceFrame>> bodyFrames; /**< Reference frames for each body. Frame names are
std::vector<ReferenceFramePtr> bodyFrames; /**< Reference frames for each body. Frame names are
the same as the body name */
// clang for some reason continuously reformats the comment on the above line no matter
// how many times you run it.
/* clang-format on */
/**< Body centered frames are aligned with the body frame, but located at a body's center of mass */
std::vector<std::shared_ptr<ReferenceFrame>> bodyCenteredFrames;
std::vector<ReferenceFramePtr> bodyCenteredFrames;
std::vector<std::shared_ptr<ReferenceFrame>> fixedBodyFrames;
std::vector<ReferenceFramePtr> fixedBodyFrames;
std::vector<unsigned int> lambda; /**< The id of the parents body */
std::vector<std::vector<unsigned int>> lambda_chain; /**< Each body's chain of parents */
......@@ -307,7 +307,8 @@ struct RDL_DLLAPI Model
/// @brief Human readable names for the bodies
std::map<std::string, unsigned int> mBodyNameMap;
std::map<std::string, ReferenceFrame*> bodyFrameMap; /**< Map containing all reference frames(i.e. both moveable and fixed) */
/**< Map containaing name -> pointer to reference frame for each frame on the robot */
std::map<std::string, ReferenceFramePtr> referenceFrameMap;
/** @brief Connects a given body to the model
*
......
......@@ -43,8 +43,8 @@ Model::Model()
gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
// state information
SpatialMotion rootBodySpatialVelocity(worldFrame.get(), worldFrame.get(), worldFrame.get(), SpatialVectorZero);
SpatialAcceleration rootBodySpatialAcceleration(worldFrame.get(), worldFrame.get(), worldFrame.get(), SpatialVectorZero);
SpatialMotion rootBodySpatialVelocity(worldFrame, worldFrame, worldFrame, SpatialVectorZero);
SpatialAcceleration rootBodySpatialAcceleration(worldFrame, worldFrame, worldFrame, SpatialVectorZero);
v.push_back(rootBodySpatialVelocity);
a.push_back(rootBodySpatialAcceleration);
......@@ -56,7 +56,7 @@ Model::Model()
X_T.push_back(SpatialTransform());
X_J.push_back(SpatialTransform());
v_J.push_back(SpatialMotion(worldFrame.get(), worldFrame.get(), worldFrame.get(), SpatialVectorZero));
v_J.push_back(SpatialMotion(worldFrame, worldFrame, worldFrame, SpatialVectorZero));
c_J.push_back(SpatialVectorZero);
// Spherical joints
......@@ -85,8 +85,8 @@ Model::Model()
bodyFrames.push_back(worldFrame); // 0th body is world
bodyCenteredFrames.push_back(worldFrame); // 0th body centered frame is world
I.push_back(SpatialInertia(worldFrame.get()));
Ib_c.push_back(SpatialInertia(worldFrame.get()));
I.push_back(SpatialInertia(worldFrame));
Ib_c.push_back(SpatialInertia(worldFrame));
Ic.push_back(RigidBodyInertia());
mBodies.push_back(root_body);
......@@ -132,7 +132,7 @@ unsigned int addBodyFixedJoint(Model& model, const unsigned int parent_id, const
}
ReferenceFramePtr fixedBodyFrame;
fixedBodyFrame.reset(new FixedReferenceFrame(body_name, model.bodyFrames[fbody.mMovableParent].get(), fbody.mParentTransform, fbody.mMovableParent));
fixedBodyFrame.reset(new FixedReferenceFrame(body_name, model.bodyFrames[fbody.mMovableParent], fbody.mParentTransform, fbody.mMovableParent));
model.fixedBodyFrames.push_back(fixedBodyFrame);
if (body_name.size() != 0)
......@@ -143,7 +143,7 @@ unsigned int addBodyFixedJoint(Model& model, const unsigned int parent_id, const
throw RdlException(msg);
}
model.mBodyNameMap[body_name] = model.mFixedBodies.size() + model.fixed_body_discriminator - 1;
model.bodyFrameMap[body_name] = fixedBodyFrame.get();
model.referenceFrameMap[body_name] = fixedBodyFrame;
}
return model.mFixedBodies.size() + model.fixed_body_discriminator - 1;
......@@ -323,16 +323,16 @@ unsigned int Model::addBody(const unsigned int parent_id, const SpatialTransform
{
parentBodyFrame = ReferenceFrame::getWorldFrame();
// movable_parent_id = 0 so this is the root body, and it's parent frame is world frame
bodyFrame.reset(new ReferenceFrame(body_name, ReferenceFrame::getWorldFrame().get(), X_lambda[X_lambda.size() - 1], true, mBodies.size()));
bodyFrame.reset(new ReferenceFrame(body_name, parentBodyFrame, X_lambda[X_lambda.size() - 1], true, mBodies.size()));
}
else
{
parentBodyFrame = bodyFrames[movable_parent_id];
bodyFrame.reset(new ReferenceFrame(body_name, bodyFrames[movable_parent_id].get(), joint_frame * movable_parent_transform, true, mBodies.size()));
bodyFrame.reset(new ReferenceFrame(body_name, parentBodyFrame, joint_frame * movable_parent_transform, true, mBodies.size()));
}
bodyFrames.push_back(bodyFrame);
bodyFrameMap[body_name] = bodyFrame.get();
referenceFrameMap[body_name] = bodyFrame;
mBodies.push_back(body);
nbodies0_vec.conservativeResize(mBodies.size());
......@@ -350,8 +350,8 @@ unsigned int Model::addBody(const unsigned int parent_id, const SpatialTransform
// state information
// Body spatial velocity. Expressed in body frame
v.push_back(SpatialMotion(bodyFrame.get(), ReferenceFrame::getWorldFrame().get(), bodyFrame.get(), SpatialVectorZero));
a.push_back(SpatialAcceleration(bodyFrame.get(), ReferenceFrame::getWorldFrame().get(), bodyFrame.get(), SpatialVectorZero));
v.push_back(SpatialMotion(bodyFrame, ReferenceFrame::getWorldFrame(), bodyFrame, SpatialVectorZero));
a.push_back(SpatialAcceleration(bodyFrame, ReferenceFrame::getWorldFrame(), bodyFrame, SpatialVectorZero));
// Joints
unsigned int prev_joint_index = mJoints.size() - 1;
......@@ -365,7 +365,7 @@ unsigned int Model::addBody(const unsigned int parent_id, const SpatialTransform
X_J.push_back(SpatialTransform());
c_J.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
v_J.push_back(SpatialMotion(bodyFrame.get(), parentBodyFrame.get(), bodyFrame.get(), joint.mJointAxes[0]));
v_J.push_back(SpatialMotion(bodyFrame, parentBodyFrame, bodyFrame, joint.mJointAxes[0]));
// workspace for joints with 3 dof
multdof3_S.push_back(Matrix63::Zero(6, 3));
......@@ -422,7 +422,7 @@ unsigned int Model::addBody(const unsigned int parent_id, const SpatialTransform
ReferenceFramePtr centerOfMassFrame;
centerOfMassFrame.reset(
new FixedReferenceFrame(body_name + "_com", bodyFrames[bodyFrames.size() - 1].get(), Xtrans(body.mCenterOfMass), bodyFrame->getMovableBodyId()));
new FixedReferenceFrame(body_name + "_com", bodyFrames[bodyFrames.size() - 1], Xtrans(body.mCenterOfMass), bodyFrame->getMovableBodyId()));
bodyCenteredFrames.push_back(centerOfMassFrame);
......@@ -431,8 +431,8 @@ unsigned int Model::addBody(const unsigned int parent_id, const SpatialTransform
std::cerr << "\033[1;31m Body " << body_name << " failed inertia validation\033[0m" << std::endl;
}
SpatialInertia bodyInertia(bodyFrames[bodyFrames.size() - 1].get(), createFromMassComInertiaC(body.mMass, body.mCenterOfMass, body.mInertia));
SpatialInertia bodyCenteredInertia(centerOfMassFrame.get(), createFromMassComInertiaC(body.mMass, Vector3dZero, body.mInertia));
SpatialInertia bodyInertia(bodyFrames[bodyFrames.size() - 1], createFromMassComInertiaC(body.mMass, body.mCenterOfMass, body.mInertia));
SpatialInertia bodyCenteredInertia(centerOfMassFrame, createFromMassComInertiaC(body.mMass, Vector3dZero, body.mInertia));
I.push_back(bodyInertia);
Ib_c.push_back(bodyCenteredInertia);
......
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