public ProvidedMassMatrixToolRigidBody(RobotSide robotSide, final FullHumanoidRobotModel fullRobotModel, double gravity,
ArmControllerParameters armControllerParameters, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry)
{
String name = robotSide.getCamelCaseNameForStartOfExpression() + "Tool";
this.registry = new YoVariableRegistry(name);
this.fullRobotModel = fullRobotModel;
this.gravity = gravity;
this.handFixedFrame = fullRobotModel.getHand(robotSide).getBodyFixedFrame();
this.handControlFrame = fullRobotModel.getHandControlFrame(robotSide);
this.elevatorFrame = fullRobotModel.getElevatorFrame();
toolFrame = new PoseReferenceFrame(name + "Frame", elevatorFrame);
RigidBodyInertia inertia = new RigidBodyInertia(toolFrame, new Matrix3d(), 0.0);
this.toolJoint = new SixDoFJoint(name + "Joint", fullRobotModel.getElevator(), fullRobotModel.getElevator().getBodyFixedFrame());
this.toolBody = new RigidBody(name + "Body", inertia, toolJoint);
objectCenterOfMass = new YoFramePoint(name + "CoMOffset", handControlFrame, registry);
objectMass = new DoubleYoVariable(name + "ObjectMass", registry);
objectForceInWorld = new YoFrameVector(name + "Force", ReferenceFrame.getWorldFrame(), registry);
this.objectCenterOfMassInWorld = new YoFramePoint(name + "CoMInWorld", ReferenceFrame.getWorldFrame(), registry);
if (yoGraphicsListRegistry != null)
{
YoGraphicsList yoGraphicsList = new YoGraphicsList(name);
YoGraphic comViz = new YoGraphicPosition(name + "CenterOfMassViz", objectCenterOfMassInWorld, 0.05, YoAppearance.Red());
yoGraphicsList.add(comViz);
YoGraphic vectorViz = new YoGraphicVector(name + "ForceViz", objectCenterOfMassInWorld, objectForceInWorld, YoAppearance.Yellow());
yoGraphicsList.add(vectorViz);
yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
}
parentRegistry.addChild(registry);
}