public UrdfNode(UrdfNode parent, Link link, RosSharp.Urdf.Joint joint)
{
_children = new List <UrdfNode>();
_parent = parent;
_link = link;
_joint = joint;
}
protected virtual Joint ExportSpecificJointData(Joint joint)
{
return(joint);
}
// 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);
}
protected virtual void ImportJointData(Joint joint)
{
}
protected virtual void AdjustMovement(Joint joint)
{
}