public SE3PIDController(String namePrefix, ReferenceFrame bodyFrame, double dt, YoSE3PIDGainsInterface gains, YoVariableRegistry registry)
{
this.bodyFrame = bodyFrame;
if (gains != null)
{
orientationController = new AxisAngleOrientationController(namePrefix, bodyFrame, dt, gains.getOrientationGains(), registry);
positionController = new EuclideanPositionController(namePrefix, bodyFrame, dt, gains.getPositionGains(), registry);
}
else
{
orientationController = new AxisAngleOrientationController(namePrefix, bodyFrame, dt, registry);
positionController = new EuclideanPositionController(namePrefix, bodyFrame, dt, registry);
}
}