/// <inheritdoc/>
protected internal override void Initialize()
{
HingeJoint joint = (HingeJoint)InspectedObject;
BuildGUI(joint, true);
drawer.AddDefault(joint, typeof(HingeJoint));
drawer.AddField("Enable limit",
() => joint.HasFlag(HingeJointFlag.Limit),
x => joint.SetFlag(HingeJointFlag.Limit, x));
drawer.AddConditional("Limit", () => joint.HasFlag(HingeJointFlag.Limit));
drawer.AddField("Enable drive",
() => joint.HasFlag(HingeJointFlag.Drive),
x => joint.SetFlag(HingeJointFlag.Drive, x));
drawer.AddConditional("Drive", () => joint.HasFlag(HingeJointFlag.Drive));
}
/// <summary>
/// Creates GUI elements for fields specific to the hinge joint.
/// </summary>
protected void BuildGUI(HingeJoint joint)
{
enableLimitField.OnChanged += x =>
{
joint.SetFlag(HingeJointFlag.Limit, x);
MarkAsModified();
ConfirmModify();
ToggleLimitFields(x);
};
enableDriveField.OnChanged += x =>
{
joint.SetFlag(HingeJointFlag.Drive, x);
MarkAsModified();
ConfirmModify();
ToggleDriveFields(x);
};
speedField.OnChanged += x =>
{
HingeJointDrive driveData = joint.Drive;
driveData.speed = x;
joint.Drive = driveData;
MarkAsModified();
};
speedField.OnFocusLost += ConfirmModify;
speedField.OnConfirmed += ConfirmModify;
forceLimitField.OnChanged += x =>
{
HingeJointDrive driveData = joint.Drive;
driveData.forceLimit = x;
joint.Drive = driveData;
MarkAsModified();
};
forceLimitField.OnFocusLost += ConfirmModify;
forceLimitField.OnConfirmed += ConfirmModify;
gearRatioField.OnChanged += x =>
{
HingeJointDrive driveData = joint.Drive;
driveData.gearRatio = x;
joint.Drive = driveData;
MarkAsModified();
};
gearRatioField.OnFocusLost += ConfirmModify;
gearRatioField.OnConfirmed += ConfirmModify;
freeSpinField.OnChanged += x =>
{
HingeJointDrive driveData = joint.Drive;
driveData.freeSpin = x;
joint.Drive = driveData;
MarkAsModified();
ConfirmModify();
};
Layout.AddElement(enableLimitField);
limitLayout = Layout.AddLayoutX();
{
limitLayout.AddSpace(10);
GUILayoutY limitContentsLayout = limitLayout.AddLayoutY();
limitGUI = new LimitAngularRangeGUI(joint.Limit, limitContentsLayout, Persistent);
limitGUI.OnChanged += (x, y) =>
{
joint.Limit = x;
joint.Limit.SetBase(y);
MarkAsModified();
};
limitGUI.OnConfirmed += ConfirmModify;
}
Layout.AddElement(enableDriveField);
driveLayout = Layout.AddLayoutX();
{
driveLayout.AddSpace(10);
GUILayoutY driveContentsLayout = driveLayout.AddLayoutY();
driveContentsLayout.AddElement(speedField);
driveContentsLayout.AddElement(forceLimitField);
driveContentsLayout.AddElement(gearRatioField);
driveContentsLayout.AddElement(freeSpinField);
}
ToggleLimitFields(joint.HasFlag(HingeJointFlag.Limit));
ToggleDriveFields(joint.HasFlag(HingeJointFlag.Drive));
base.BuildGUI(joint, true);
}
// TODO
// * Support Urdf axis that aren't directly aligned with the
// X, Y, or Z axis
/// <summary>
/// <para>ConfigureJoint</para>
/// Configures the Godot Generic6DOFJoint
/// properties to match what the Urdf joint
/// contains.
/// </summary>
/// <param name="base_joint">Urdf Joint specifications.</param>
/// <returns>Godot joint matching specs.</returns>
private Godot.Joint ConfigureJoint(RosSharp.Urdf.Joint base_joint)
{
// The Urdf joint axis specifies the axis of rotation for revolute joints,
// axis of translation for prismatic joints, and the surface normal
// for planar joints.
// If it's not specified accessing it will error out, so we need to
// manually specify the default (X-axis).
double[] j_axis;
try
{
j_axis = base_joint.axis.xyz;
}
catch
{
j_axis = new double[] { 1.0, 0.0, 0.0 };
}
GD.Print(base_joint.name + " axis: " + j_axis[0] + " " + j_axis[1] + " " + j_axis[2]);
JointMaker mkr = new JointMaker();
// Type comments taken from https://wiki.ros.org/urdf/XML/joint
switch (base_joint.type)
{
case "revolute":
// A hinge joint that rotates along the axis and has a
// limited range specified by the upper and lower limits.
// HingeJoint revJoint = mkr.CreateHingeJoint();
// // HingeJoint revJoint = new HingeJoint();
// // revJoint.rot/
// GD.Print("setting hingejoint flags");
// revJoint.SetFlag(HingeJoint.Flag.UseLimit, true);
// revJoint.SetFlag(HingeJoint.Flag.EnableMotor, true);
// revJoint.SetParam(HingeJoint.Param.MotorTargetVelocity, 0F);
// revJoint.SetParam(HingeJoint.Param.MotorMaxImpulse, 1024F);
// revJoint.SetParam(HingeJoint.Param.Bias, 1F);
// revJoint.SetParam(HingeJoint.Param.LimitLower, (float)base_joint.limit.lower * (180F / (float)Math.PI));
// revJoint.SetParam(HingeJoint.Param.LimitUpper, (float)base_joint.limit.upper * (180F / (float)Math.PI));
// return revJoint;
case "continuous":
// a continuous hinge joint that rotates around the axis
// and has no upper and lower limits.
HingeJoint contJoint = new HingeJoint();
contJoint.SetFlag(HingeJoint.Flag.UseLimit, false);
contJoint.SetFlag(HingeJoint.Flag.EnableMotor, true);
contJoint.SetParam(HingeJoint.Param.MotorTargetVelocity, 0F);
return(contJoint);
case "prismatic":
// a sliding joint that slides along the axis, and has a
// limited range specified by the upper and lower limits.
SliderJoint slideJoint = new SliderJoint();
slideJoint.SetParam(SliderJoint.Param.LinearLimitLower, (float)base_joint.limit.lower);
slideJoint.SetParam(SliderJoint.Param.LinearLimitUpper, (float)base_joint.limit.upper);
return(slideJoint);
case "fixed":
// This is not really a joint because it cannot move.
// All degrees of freedom are locked. This type of joint
// does not require the axis, calibration, dynamics,
// limits or safety_controller.
Generic6DOFJoint pinJoint = new Generic6DOFJoint();
pinJoint.SetFlagX(Generic6DOFJoint.Flag.EnableAngularLimit, true);
pinJoint.SetFlagY(Generic6DOFJoint.Flag.EnableAngularLimit, true);
pinJoint.SetFlagZ(Generic6DOFJoint.Flag.EnableAngularLimit, true);
pinJoint.SetFlagX(Generic6DOFJoint.Flag.EnableLinearLimit, true);
pinJoint.SetFlagY(Generic6DOFJoint.Flag.EnableLinearLimit, true);
pinJoint.SetFlagZ(Generic6DOFJoint.Flag.EnableLinearLimit, true);
return(pinJoint);
case "floating":
// This joint allows motion for all 6 degrees of freedom.
Generic6DOFJoint genJoint = new Generic6DOFJoint();
genJoint.SetFlagX(Generic6DOFJoint.Flag.EnableAngularLimit, false);
genJoint.SetFlagY(Generic6DOFJoint.Flag.EnableAngularLimit, false);
genJoint.SetFlagZ(Generic6DOFJoint.Flag.EnableAngularLimit, false);
genJoint.SetFlagX(Generic6DOFJoint.Flag.EnableLinearLimit, false);
genJoint.SetFlagY(Generic6DOFJoint.Flag.EnableLinearLimit, false);
genJoint.SetFlagZ(Generic6DOFJoint.Flag.EnableLinearLimit, false);
return(genJoint);
case "planar":
// This joint allows motion in a plane perpendicular to the axis.
Generic6DOFJoint planJoint = new Generic6DOFJoint();
if (j_axis[0] == 1.0)
{
planJoint.SetParamY(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamY(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
planJoint.SetParamZ(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamZ(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
}
if (j_axis[1] == 1.0)
{
planJoint.SetParamY(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamY(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
planJoint.SetParamX(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamX(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
}
if (j_axis[2] == 1.0)
{
planJoint.SetParamX(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamX(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
planJoint.SetParamZ(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamZ(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
}
return(planJoint);
default:
GD.Print("testing: " + base_joint.type);
break;
}
return(null);
}
