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