public DummyOneDegreeOfFreedomJoint(String jname, Vector3d offset, Robot rob, Vector3d u_hat) {
super(jname, offset, rob);
physics = new DummyOneDegreeOfFreedomJointPhysics(this);
physics.u_i = new Vector3d();
physics.u_i.set(u_hat);
physics.u_i.normalize();
YoVariableRegistry registry = rob.getRobotsYoVariableRegistry();
q = new DoubleYoVariable("q_" + jname, "PinJoint angle", registry);
qd = new DoubleYoVariable("qd_" + jname, "PinJoint anglular velocity", registry);
qdd = new DoubleYoVariable("qdd_" + jname, "PinJoint angular acceleration", registry);
tau = new DoubleYoVariable("tau_" + jname, "PinJoint torque", registry);
this.setPinTransform3D(this.jointTransform3D, physics.u_i); // jaxis);
}
public GroundContactPoint(String name, Vector3d offset, Robot robot) {
this(name, offset, robot.getRobotsYoVariableRegistry());
}
public GroundContactPoint(String name, Robot robot) {
this(name, null, robot.getRobotsYoVariableRegistry());
}
public FloatingJoint(
String jname,
String varName,
Vector3d offset,
Robot rob,
boolean createYawPitchRollYoVariable) {
super(jname, offset, rob, 6);
physics = new FloatJointPhysics(this);
YoVariableRegistry registry = rob.getRobotsYoVariableRegistry();
this.createYawPitchRollYoVariable = createYawPitchRollYoVariable;
if (varName == null) {
varName = "";
} else if (!varName.isEmpty()) {
varName += "_";
}
q_x = new DoubleYoVariable("q_" + varName + "x", "FloatingJoint x position", registry);
q_y = new DoubleYoVariable("q_" + varName + "y", "FloatingJoint y position", registry);
q_z = new DoubleYoVariable("q_" + varName + "z", "FloatingJoint z position", registry);
qd_x = new DoubleYoVariable("qd_" + varName + "x", "FloatingJoint x velocity", registry);
qd_y = new DoubleYoVariable("qd_" + varName + "y", "FloatingJoint y velocity", registry);
qd_z = new DoubleYoVariable("qd_" + varName + "z", "FloatingJoint z velocity", registry);
qdd_x = new DoubleYoVariable("qdd_" + varName + "x", "FloatingJoint x acceleration", registry);
qdd_y = new DoubleYoVariable("qdd_" + varName + "y", "FloatingJoint yx acceleration", registry);
qdd_z = new DoubleYoVariable("qdd_" + varName + "z", "FloatingJoint z acceleration", registry);
q_qs =
new DoubleYoVariable(
"q_" + varName + "qs", "FloatingJoint orientation quaternion qs", registry);
q_qs.set(1.0);
q_qx =
new DoubleYoVariable(
"q_" + varName + "qx", "FloatingJoint orientation quaternion qx", registry);
q_qy =
new DoubleYoVariable(
"q_" + varName + "qy", "FloatingJoint orientation quaternion qy", registry);
q_qz =
new DoubleYoVariable(
"q_" + varName + "qz", "FloatingJoint orientation quaternion qz", registry);
qd_wx =
new DoubleYoVariable(
"qd_" + varName + "wx", "FloatingJoint rotational velocity about x", registry);
qd_wy =
new DoubleYoVariable(
"qd_" + varName + "wy", "FloatingJoint rotational velocity about y", registry);
qd_wz =
new DoubleYoVariable(
"qd_" + varName + "wz", "FloatingJoint rotational velocity about z", registry);
qdd_wx =
new DoubleYoVariable(
"qdd_" + varName + "wx", "FloatingJoint rotational acceleration about x", registry);
qdd_wy =
new DoubleYoVariable(
"qdd_" + varName + "wy", "FloatingJoint rotational acceleration about y", registry);
qdd_wz =
new DoubleYoVariable(
"qdd_" + varName + "wz", "FloatingJoint rotational acceleration about z", registry);
if (createYawPitchRollYoVariable) {
q_yaw =
new DoubleYoVariable("q_" + varName + "yaw", "FloatingJoint yaw orientation", registry);
q_pitch =
new DoubleYoVariable(
"q_" + varName + "pitch", "FloatingJoint pitch orientation", registry);
q_roll =
new DoubleYoVariable("q_" + varName + "roll", "FloatingJoint roll orientation", registry);
} else {
q_yaw = null;
q_pitch = null;
q_roll = null;
}
this.setFloatingTransform3D(this.jointTransform3D);
physics.u_i = null;
}
