Trouble with IK joint limitations

[DJSures]

I have a 4 DOF arm with a sphere as the end effector. I'm only using the MonoGame and Bepu meshes to see the IK result visually. The end goal is to use the BEPUik class standalone for inverse kinematics. To visualize the result of the IK solver (bones and joints), I created a sample project. I used InverseKinematicsTestDemo2 as inspiration because it uses the BEPUik class.

Result where target position is out of reach of arm...

Result where target position is reachable of arm...

Here are the issues I am encountering, including what I expect to happen. I'm missing something and want a second set of eyes to help me.

1. I need help figuring out how to add limits to the IKRevoluteJoint. There are a few "Limit" joints, but I'm unsure which will work for the Revolute.

2. I am trying to force the distance of bones anchored to each other so they're "Attached." The bones are separated when the target position is out of reach. I expect the end effector to point toward the target position even if it cannot reach it. Please look at the above photo of how each joint is separated. On the contrary, the bones overlap if the target position is close.

3. The bones do not seem aligned toward the end effector. As if some bones are not aligned along the "arm." They seem to point along the arm. Please take a look at the image of how the three bones are not aligned with each other.

using System.Collections.Generic;
using BEPUik;
using BEPUphysics;
using BEPUphysics.Entities;
using BEPUphysics.Entities.Prefabs;
using BEPUutilities;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Matrix = Microsoft.Xna.Framework.Matrix;
using Vector3 = BEPUutilities.Vector3;

namespace My3DApp {

  // Inspired by InverseKinematicsTestDemo2
  // The goal is to point the end effector toward the Target Position
  public class TheApp : Game {

    public KeyboardState KeyboardState;
    public MouseState    MouseState;
    public Camera        Camera;

    GraphicsDeviceManager graphics;
    Space                 space;

    // List of the bones we're using for the IK solver
    List<Bone>   bones    = new List<Bone>();

    // List of the entities we've added to the space
    // We force the position of these by the bones position after the IK Solver
    List<Entity> entities = new List<Entity>();

    public TheApp() {

      IsMouseVisible = true;

      graphics = new GraphicsDeviceManager(this);
      graphics.PreferredBackBufferWidth = 1280;
      graphics.PreferredBackBufferHeight = 720;

      Content.RootDirectory = "Content";
    }

    protected override void Initialize() {

      Camera = new Camera(this, new Vector3(0, 3, 10), 3);

      base.Initialize();
    }

    protected override void LoadContent() {

      var cubeModel = Content.Load<Model>("cube");
      var cylinderModel = Content.Load<Model>("cylinder");
      var sphereModel = Content.Load<Model>("360 thick sphere");

      space = new Space();

      // ---------------------------------------------------------------

      var eGround = new Box(new Vector3(0, -1, 0), 15, 0.5f, 15);
      entities.Add(eGround);

      var bGround = new Bone(eGround.Position, eGround.Orientation, eGround.Width, eGround.Height);
      bGround.Pinned = true;
      bones.Add(bGround);

      // ---------------------------------------------------------------

      var eCyl1 = new Box(new Vector3(0, 0, 0), 1, 2, 1);
      entities.Add(eCyl1);

      var bCyl1 = new Bone(eCyl1.Position, eCyl1.Orientation, eCyl1.Width, eCyl1.Height);
      bones.Add(bCyl1);

      // ---------------------------------------------------------------

      var eCyl2 = new Cylinder(new Vector3(0, 2, 0), 2, 0.2f);
      entities.Add(eCyl2);

      var bCyl2 = new Bone(eCyl2.Position, eCyl2.Orientation, eCyl2.Radius, eCyl2.Height);
      bones.Add(bCyl2);

      // ---------------------------------------------------------------

      var eCyl3 = new Cylinder(new Vector3(0, 4, 0), 2, 0.2f);
      entities.Add(eCyl3);

      var bCyl3 = new Bone(eCyl3.Position, eCyl3.Orientation, eCyl3.Radius, eCyl3.Height);
      bones.Add(bCyl3);

      // ---------------------------------------------------------------

      var eCyl4 = new Cylinder(new Vector3(0, 6, 0), 2, 0.2f);
      entities.Add(eCyl4);

      var bCyl4 = new Bone(eCyl4.Position, eCyl4.Orientation, eCyl4.Radius, eCyl4.Height);
      bones.Add(bCyl4);

      // ---------------------------------------------------------------

      var eEndEffector = new Sphere(new Vector3(0, 8, 0), 0.5f);
      entities.Add(eEndEffector);

      var bEndEffector = new Bone(eEndEffector.Position, eEndEffector.Orientation, eEndEffector.Radius, eEndEffector.Radius);
      bones.Add(bEndEffector);

      // ---------------------------------------------------------------

      new IKRevoluteJoint(bGround, bCyl1, Vector3.Up);
//      new IKBallSocketJoint(bGround, bCyl1, bCyl1.Position);
      //new IKAngularJoint(bGround, bCyl1);
      new IKDistanceJoint(bGround, bCyl1, bCyl1.Position, bCyl1.Position);

      new IKRevoluteJoint(bCyl1, bCyl2, Vector3.Forward);
//      new IKBallSocketJoint(bCyl1, bCyl2, bCyl2.Position);
      //new IKAngularJoint(bCyl1, bCyl2);
      new IKDistanceJoint(bCyl1, bCyl2, bCyl2.Position, bCyl2.Position);

      new IKRevoluteJoint(bCyl2, bCyl3, Vector3.Forward);
//      new IKBallSocketJoint(bCyl2, bCyl3, bCyl3.Position);
      //new IKAngularJoint(bCyl1, bCyl2);
      new IKDistanceJoint(bCyl2, bCyl3, bCyl3.Position, bCyl3.Position);

      new IKRevoluteJoint(bCyl3, bCyl4, Vector3.Forward);
      //      new IKBallSocketJoint(bCyl2, bCyl3, bCyl3.Position);
      //new IKAngularJoint(bCyl1, bCyl2);
      new IKDistanceJoint(bCyl3, bCyl4, bCyl4.Position, bCyl4.Position);

      new IKRevoluteJoint(bCyl4, bEndEffector, Vector3.Forward);
//      new IKBallSocketJoint(bCyl3, bEndEffector, bEndEffector.Position);
      //new IKAngularJoint(bCyl1, bCyl2);
      new IKDistanceJoint(bCyl4, bEndEffector, bEndEffector.Position, bEndEffector.Position);

      // ---------------------------------------------------------------

      // Add all entities to the space and set the rendererererer
      foreach (Entity e in entities) {

        space.Add(e);

        if (e is Box) {

          var obj = e as Box;

          Matrix scaling = Matrix.CreateScale(obj.Width, obj.Height, obj.Length);

          Components.Add(new EntityModel(e, cubeModel, scaling, this));
        } else if (e is Cylinder) {

          var obj = e as Cylinder;

          Matrix scaling = Matrix.CreateScale(obj.Radius, obj.Height, obj.Radius);

          Components.Add(new EntityModel(e, cylinderModel, scaling, this));
        } else if (e is Sphere) {

          var obj = e as Sphere;

          Matrix scaling = Matrix.CreateScale(0.01f, 0.01f, 0.01f);

          Components.Add(new EntityModel(e, sphereModel, scaling, this));
        }
      }

      base.LoadContent();
    }

    protected override void Update(GameTime gameTime) {

      KeyboardState = Keyboard.GetState();
      MouseState = Mouse.GetState();

      Camera.Update((float)gameTime.ElapsedGameTime.TotalSeconds);

      if (KeyboardState.IsKeyDown(Keys.Escape)) {

        Exit();

        return;
      }

      DragControl dc = new DragControl();
      dc.LinearMotor.TargetBone = bones[bones.Count - 1];
      dc.LinearMotor.Rigidity = 16;
      dc.LinearMotor.LocalOffset = new Vector3(0, 0, 0);
      dc.LinearMotor.TargetPosition = new Vector3(3, 3, 0);

      var controls = new List<Control>();
      controls.Add(dc);

      var solver = new IKSolver();
      solver.ActiveSet.UseAutomass = true;

      // how many times the solver iterates over the controls in each update.
      // If this is set too low, the solver might not have enough iterations to find a solution.
      solver.ControlIterationCount = 100;

      solver.FixerIterationCount = 0;

      // how many times the solver iterates over the velocities in each update.
      // If this is set too low, the solver might not be able to accurately calculate the velocities
      solver.VelocitySubiterationCount = 100;

      solver.Solve(controls);

      var tipLocation = bones[bones.Count - 1].Position + Matrix3x3.CreateFromQuaternion(bones[bones.Count - 1].Orientation).Up * 0.5f;

      for (int i = 0; i < bones.Count; i++) {

        entities[i].Position = bones[i].Position;
        entities[i].Orientation = bones[i].Orientation;
      }

      space.Update();

      base.Update(gameTime);
    }

    protected override void Draw(GameTime gameTime) {

      GraphicsDevice.Clear(Color.CornflowerBlue);

      base.Draw(gameTime);
    }
  }
}

[RossNordby]

IKDistanceJoint tries to maintain the initial distance between points attached to each body. In the code provided, the attachment points are both specified to be at the same point, which means the distance constraint is targeting 0 distance. This won't work well; the constraint needs a valid axis to work on and targeting 0 distance pushes the constraint towards an unstable singularity. Use the IKBallSocketJoint instead.

Also, the joints in the snippet are configured with anchors at entity positions. Entity positions are at the center of the entity, not on the surface. You'll want to offset the anchor like in the BuildRoboArmThing sample from the InverseKinematicsTestDemo.

When trying to check if a configuration is correct and to debug stability issues, try setting the Solver.TimeStepDuration to a smaller value and using more steps to compensate. For example:

            solver.TimeStepDuration = .01f;
            solver.ControlIterationCount = 1000;
            solver.FixerIterationCount = 1000;
            solver.VelocitySubiterationCount = 1;

Really high iteration counts will be expensive, but it's only necessary as long as you're diagnosing issues. It can be configured for performance later. 

There are a few "Limit" joints, but I'm unsure which will work for the Revolute.

The IKSwingLimit's the simplest and easiest.

[DJSures]

Thank you for the assistance - I have made significant progress with your advice.

1. Because the Position is the center of entities/bones, I assume the "dc.LinearMotor.TargetPosition" is also the same. So, would I set the "dc.LinearMotor.LocalOffset" with the half-height of the end effector? That way, will the end of the end effector be at the target position?

// offset so the target position is the end of the end effector. Otherwise, it's the center of the end effector
dc.LinearMotor.LocalOffset = new Vector3(0, bones[bones.Count - 1].HalfHeight, 0); 

Here's an updated screenshot to see the progress.

Lastly, here's my updated code for reference...

using System.Collections.Generic;
using BEPUik;
using BEPUphysics;
using BEPUphysics.Entities;
using BEPUphysics.Entities.Prefabs;
using BEPUutilities;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Matrix = Microsoft.Xna.Framework.Matrix;
using Vector3 = BEPUutilities.Vector3;

namespace My3DApp {

  // Inspired by InverseKinematicsTestDemo2
  // The goal is to point the end effector toward the Target Position
  public class TheApp : Game {

    public KeyboardState KeyboardState;
    public MouseState    MouseState;
    public Camera        Camera;

    GraphicsDeviceManager graphics;
    Space                 space;

    // List of the bones we're using for the IK solver
    List<Bone> bones = new List<Bone>();

    // List of the entities we've added to the space
    // We force the position of these by the bones position after the IK Solver
    List<Entity> entities = new List<Entity>();

    public TheApp() {

      IsMouseVisible = true;

      graphics = new GraphicsDeviceManager(this);
      graphics.PreferredBackBufferWidth = 1280;
      graphics.PreferredBackBufferHeight = 720;

      Content.RootDirectory = "Content";
    }

    protected override void Initialize() {

      Camera = new Camera(this, new Vector3(0, 3, 10), 3);

      base.Initialize();
    }

    protected override void LoadContent() {

      var cubeModel = Content.Load<Model>("cube");
      var cylinderModel = Content.Load<Model>("cylinder");

      space = new Space();

      // ---------------------------------------------------------------

      // top: 0
      var eGround = new Box(new Vector3(0, -1, 0), 15, 2, 15);
      entities.Add(eGround);

      var bGround = new Bone(eGround.Position, eGround.Orientation, eGround.Width, eGround.Height);
      bGround.Pinned = true;
      bones.Add(bGround);

      // ---------------------------------------------------------------

      // top: 2
      var eCyl1 = new Box(new Vector3(0, 1, 0), 1, 2, 1);
      entities.Add(eCyl1);

      var bCyl1 = new Bone(eCyl1.Position, eCyl1.Orientation, eCyl1.Width, eCyl1.Height);
      bones.Add(bCyl1);

      // ---------------------------------------------------------------

      // top: 4
      var eCyl2 = new Cylinder(new Vector3(0, 3, 0), 2, 0.2f);
      entities.Add(eCyl2);

      var bCyl2 = new Bone(eCyl2.Position, eCyl2.Orientation, eCyl2.Radius, eCyl2.Height);
      bones.Add(bCyl2);

      // ---------------------------------------------------------------

      // top: 6
      var eCyl3 = new Cylinder(new Vector3(0, 5, 0), 2, 0.2f);
      entities.Add(eCyl3);

      var bCyl3 = new Bone(eCyl3.Position, eCyl3.Orientation, eCyl3.Radius, eCyl3.Height);
      bones.Add(bCyl3);

      // ---------------------------------------------------------------

      // top: 8
      var eCyl4 = new Cylinder(new Vector3(0, 7, 0), 2, 0.2f);
      entities.Add(eCyl4);

      var bCyl4 = new Bone(eCyl4.Position, eCyl4.Orientation, eCyl4.Radius, eCyl4.Height);
      bones.Add(bCyl4);

      // ---------------------------------------------------------------

      // top: 10
      var eCyl5 = new Cylinder(new Vector3(0, 9, 0), 2, 0.2f);
      entities.Add(eCyl5);

      var bCyl5 = new Bone(eCyl5.Position, eCyl5.Orientation, eCyl5.Radius, eCyl5.Height);
      bones.Add(bCyl5);

      // ---------------------------------------------------------------

      new IKRevoluteJoint(bGround, bCyl1, Vector3.Up);
      new IKBallSocketJoint(bGround, bCyl1, bGround.Position + new Vector3(0, bGround.HalfHeight, 0));

      new IKRevoluteJoint(bCyl1, bCyl2, Vector3.Forward);
      new IKBallSocketJoint(bCyl1, bCyl2, bCyl1.Position + new Vector3(0, bCyl1.HalfHeight, 0));

      new IKRevoluteJoint(bCyl2, bCyl3, Vector3.Forward);
      new IKBallSocketJoint(bCyl2, bCyl3, bCyl2.Position + new Vector3(0, bCyl2.HalfHeight, 0));

      new IKRevoluteJoint(bCyl3, bCyl4, Vector3.Forward);
      new IKBallSocketJoint(bCyl3, bCyl4, bCyl3.Position + new Vector3(0, bCyl3.HalfHeight, 0));

      new IKRevoluteJoint(bCyl4, bCyl5, Vector3.Forward);
      new IKBallSocketJoint(bCyl4, bCyl5, bCyl4.Position + new Vector3(0, bCyl4.HalfHeight, 0));

      // ---------------------------------------------------------------

      // Add all entities to the space and set the rendererererer
      foreach (Entity e in entities) {

        space.Add(e);

        if (e is Box) {

          var obj = e as Box;

          Matrix scaling = Matrix.CreateScale(obj.Width, obj.Height, obj.Length);

          Components.Add(new EntityModel(e, cubeModel, scaling, this));
        } else if (e is Cylinder) {

          var obj = e as Cylinder;

          Matrix scaling = Matrix.CreateScale(obj.Radius, obj.Height, obj.Radius);

          Components.Add(new EntityModel(e, cylinderModel, scaling, this));
        }
      }

      base.LoadContent();
    }

    protected override void Update(GameTime gameTime) {

      KeyboardState = Keyboard.GetState();
      MouseState = Mouse.GetState();

      if (KeyboardState.IsKeyDown(Keys.Escape)) {

        Exit();

        return;
      }

      DragControl dc = new DragControl();
      dc.LinearMotor.TargetBone = bones[bones.Count - 1];
      dc.LinearMotor.Rigidity = 16;
      dc.LinearMotor.LocalOffset = new Vector3(0, bones[bones.Count - 1].HalfHeight, 0); // offset so the target position is the end of the end effector, otherwise it's the center of the end effector
      dc.LinearMotor.TargetPosition = new Vector3(-5, 4, 5);

      var controls = new List<Control>();
      controls.Add(dc);

      var solver = new IKSolver();
      solver.ActiveSet.UseAutomass = true;
      solver.TimeStepDuration = .01f;
      solver.ControlIterationCount = 1000;
      solver.FixerIterationCount = 1000;
      solver.VelocitySubiterationCount = 1;
      solver.Solve(controls);

      var tipLocation = bones[bones.Count - 1].Position + Matrix3x3.CreateFromQuaternion(bones[bones.Count - 1].Orientation).Up * 0.5f;

      for (int i = 0; i < bones.Count; i++) {

        entities[i].Position = bones[i].Position;
        entities[i].Orientation = bones[i].Orientation;
      }

      space.Update();

      Camera.Update((float)gameTime.ElapsedGameTime.TotalSeconds);

      base.Update(gameTime);
    }

    protected override void Draw(GameTime gameTime) {

      GraphicsDevice.Clear(Color.CornflowerBlue);

      base.Draw(gameTime);
    }
  }
}

[RossNordby]

So, would I set the "dc.LinearMotor.LocalOffset" with the half-height of the end effector?

Yup!

[DJSures]

Ross, this is working great - thank you. I have it controlling a robot arm using the IK class (without any rendering or physics class). I have experienced one issue that occurs when the X-axis is dropped below 0.15f.

Here is the robot arm at X position 15
dc.LinearMotor.TargetPosition = new Vector3(0.15f, 0.10f, 0.0f);

Here is the robot arm at X position 14
dc.LinearMotor.TargetPosition = new Vector3(0.14f, 0.10f, 0.0f);

Here's more code for reference...

using System;
using System.Collections.Generic;
using BEPUik;
using BEPUphysics;
using BEPUphysics.Entities;
using BEPUphysics.Entities.Prefabs;
using BEPUutilities;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Matrix = Microsoft.Xna.Framework.Matrix;
using Vector3 = BEPUutilities.Vector3;
using Quaternion = BEPUutilities.Quaternion;
using MathHelper = BEPUutilities.MathHelper;
using BEPUphysics.Character;

namespace My3DApp {

  // Inspired by InverseKinematicsTestDemo2
  // The goal is to point the end effector toward the Target Position
  public class TheApp : Game {

    public KeyboardState KeyboardState;
    public MouseState    MouseState;
    public Camera        Camera;

    GraphicsDeviceManager graphics;
    Space                 space;

    // List of the bones we're using for the IK solver
    List<Bone> bones = new List<Bone>();

    // List of the entities we've added to the space
    // We force the position of these by the bones position after the IK Solver
    List<Entity> entities = new List<Entity>();

    public TheApp() {

      IsMouseVisible = true;

      graphics = new GraphicsDeviceManager(this);
      graphics.PreferredBackBufferWidth = 1280;
      graphics.PreferredBackBufferHeight = 720;

      Content.RootDirectory = "Content";
    }

    protected override void Initialize() {

      Camera = new Camera(this, new Vector3(0, 0.1f, 0.7f), 1);

      base.Initialize();
    }

    protected override void LoadContent() {

      var cubeModel = Content.Load<Model>("cube");
      var cylinderModel = Content.Load<Model>("cylinder");

      // ---------------------------------------------------------------
      // All units are in meters
      // ---------------------------------------------------------------

      space = new Space();
      float top = 0;
      float cylRadius = 0.01f;

      // ---------------------------------------------------------------
      // The ground

      var bGround = new Bone(new Vector3(0, -0.05f, 0), Quaternion.Identity, 1, 0.1f);
      bGround.Pinned = true;
      bones.Add(bGround);
      top = bGround.Position.Y + bGround.HalfHeight;
      
      var eGround = new Box(bGround.Position, bGround.Radius / 2, bGround.Height, bGround.Radius / 2);
      entities.Add(eGround);

      // ---------------------------------------------------------------
      // The box that rotates the arm along the X axis

      float box1Height = 0.0765f;
      var bBox1 = new Bone(new Vector3(0, top + (box1Height / 2), 0), Quaternion.Identity, 0.1f, box1Height);
      bones.Add(bBox1);
      top += box1Height;

      var eBox1 = new Box(bBox1.Position, bBox1.Radius / 2, bBox1.Height, bBox1.Radius / 2);
      entities.Add(eBox1);

      // ---------------------------------------------------------------
      // The first arm

      float eCyl1Height = 0.128f;
      var bCyl1 = new Bone(new Vector3(0, top + (eCyl1Height / 2), 0), Quaternion.Identity, cylRadius, eCyl1Height);
      bones.Add(bCyl1);
      top += eCyl1Height;

      var eCyl1 = new Cylinder(bCyl1.Position, bCyl1.Height, bCyl1.Radius);
      entities.Add(eCyl1);

      // ---------------------------------------------------------------
      // The second arm
      // offset from Y axis by 0.024f

      float eCyl2Height = 0.124f;
      var bCyl2 = new Bone(new Vector3(0.024f, top + (eCyl2Height / 2), 0), Quaternion.Identity, cylRadius, eCyl2Height);
      bones.Add(bCyl2);
      top += eCyl2Height;

      var eCyl2 = new Cylinder(bCyl2.Position, bCyl2.Height, bCyl2.Radius);
      entities.Add(eCyl2);

      // ---------------------------------------------------------------
      // The end effector
      // offset from Y axis by 0.024f

      float eCyl3Height = 0.146f;
      var bCyl3 = new Bone(new Vector3(0.024f, top + (eCyl3Height / 2), 0), Quaternion.Identity, cylRadius, eCyl3Height);
      bones.Add(bCyl3);
      top += eCyl3Height;

      var eCyl3 = new Cylinder(bCyl3.Position, bCyl3.Height, bCyl3.Radius);
      entities.Add(eCyl3);

      // ---------------------------------------------------------------

      // Box that rotates around the X axis
      new IKRevoluteJoint(bGround, bBox1, Vector3.Up);
      new IKBallSocketJoint(bGround, bBox1, bBox1.Position - new Vector3(0, bBox1.HalfHeight, 0));

      // Box -> First Arm
      new IKRevoluteJoint(bBox1, bCyl1, Vector3.Forward);
      new IKBallSocketJoint(bBox1, bCyl1, bCyl1.Position - new Vector3(0, bCyl1.HalfHeight, 0));

      // First Arm -> Second Arm
      new IKRevoluteJoint(bCyl1, bCyl2, Vector3.Forward);
      new IKBallSocketJoint(bCyl1, bCyl2, bCyl2.Position - new Vector3(0, bCyl2.HalfHeight, 0));

      // Second Arm -> End effector
      new IKRevoluteJoint(bCyl2, bCyl3, Vector3.Forward);
      new IKBallSocketJoint(bCyl2, bCyl3, bCyl3.Position - new Vector3(0, bCyl3.HalfHeight, 0));

      // ---------------------------------------------------------------

      // Add all entities to the space and set the rendererererer
      foreach (Entity e in entities) {

        space.Add(e);

        if (e is Box) {

          var obj = e as Box;

          Matrix scaling = Matrix.CreateScale(obj.Width, obj.Height, obj.Length);

          Components.Add(new EntityModel(e, cubeModel, scaling, this));
        } else if (e is Cylinder) {

          var obj = e as Cylinder;

          Matrix scaling = Matrix.CreateScale(obj.Radius, obj.Height, obj.Radius);

          Components.Add(new EntityModel(e, cylinderModel, scaling, this));
        }
      }

      base.LoadContent();
    }

    protected override void Update(GameTime gameTime) {

      KeyboardState = Keyboard.GetState();
      MouseState = Mouse.GetState();

      if (KeyboardState.IsKeyDown(Keys.Escape)) {

        Exit();

        return;
      }

      DragControl dc = new DragControl();
      dc.LinearMotor.TargetBone = bones[bones.Count - 1];
      dc.LinearMotor.Rigidity = 16;
      dc.LinearMotor.LocalOffset = new Vector3(0, bones[bones.Count - 1].HalfHeight, 0); // offset so the target position is the end of the end effector, otherwise without setting this the target position would be the center of the end effector
      dc.LinearMotor.TargetPosition = new Vector3(0.10f, 0.10f, 0.0f); // Meters

      var controls = new List<Control>();
      controls.Add(dc);

      var solver = new IKSolver();
      solver.ActiveSet.UseAutomass = true;
      solver.TimeStepDuration = .01f;
      solver.ControlIterationCount = 1000;
      solver.FixerIterationCount = 1000;
      solver.VelocitySubiterationCount = 1;
      solver.Solve(controls);

      var tipLocation = bones[bones.Count - 1].Position + Matrix3x3.CreateFromQuaternion(bones[bones.Count - 1].Orientation).Up * 0.5f;

      for (int i = 0; i < bones.Count; i++) {

        entities[i].Position = bones[i].Position;
        entities[i].Orientation = bones[i].Orientation;
      }

      space.Update();

      Camera.Update((float)gameTime.ElapsedGameTime.TotalSeconds);

      base.Update(gameTime);
    }

    protected override void Draw(GameTime gameTime) {

      GraphicsDevice.Clear(Color.CornflowerBlue);

      base.Draw(gameTime);
    }
  }
}

[RossNordby]

I notice space.Update() is being called after setting the entity poses to match the bone poses, and that rendering is based on the dynamics simulation results through the EntityModel. If the IK-produced poses result in penetration between entities, that forward simulation step will introduce weird behaviors.

Could also be some remaining failure to converge in the IK solver; you could make the IKSolver's settings even more extreme to check for thatt (like 1e-3f or 1e-4f TimeStepDuration with 10000 or 100000 control iteration count).

[DJSures]

I appreciate your suggestions; however, adjusting the step duration or iteration counts did not resolve the issue. I had even changed the iterations to 1,000,000 with similar results.

I'd like to clarify that I don't believe the EntityModel is the root cause of this problem. The issue persists regardless of whether the entities are specified or not.

In my actual application, I exclusively employ the IK solver as I don't rely on the entity framework for the final output. My primary use case for this exercise doesn't involve GUI rendering, so the entity framework (i.e., space.update()) is not utilized in my core application. The code I'm sharing here exclusively employs your entity framework for simulation purposes, making it easier for you to replicate and comprehend my approach. I've separated the IK component of my code for use without entities in a library that manages the physical robot hardware. Specifically, the only projects from this library that I'm utilizing are BEPUik and BEPUutilities.

It's strange that any X value less than 0.15 produces these results. The joints mangle once any value is less than 0.15

Here is a result with the following solver settings..

      DragControl dc = new DragControl();
      dc.LinearMotor.TargetBone = bones[bones.Count - 1];
      dc.LinearMotor.Rigidity = 16;
      dc.LinearMotor.LocalOffset = new Vector3(0, bones[bones.Count - 1].HalfHeight, 0);
      dc.LinearMotor.TargetPosition = new Vector3(0.10f, 0.05f, 0.0f); 

      var solver = new IKSolver();
      solver.ActiveSet.UseAutomass = true;
      solver.TimeStepDuration = 1e-4f; //.01f;
      solver.ControlIterationCount = 100000;
      solver.FixerIterationCount = 100000;
      solver.VelocitySubiterationCount = 1;
      solver.Solve(controls);

Would adding rotation limitations on the joints help?

[DJSures]

I want to be considerate of your inbox, but I'd like to share that I've achieved a target position of 0.09 using the provided values. However, it's worth noting that going below 0.09 with these parameters leads to issues with joint integrity.

      DragControl dc = new DragControl();
      dc.LinearMotor.TargetBone = bones[bones.Count - 1];
      dc.LinearMotor.Rigidity = 1000; <-------------------------------- (changed from 16)
      dc.LinearMotor.LocalOffset = new Vector3(0, bones[bones.Count - 1].HalfHeight, 0); 
      dc.LinearMotor.TargetPosition = new Vector3(0.09f, 0.02f, 0.0f); 

      var controls = new List<Control>();
      controls.Add(dc);

      var solver = new IKSolver();
      solver.ActiveSet.UseAutomass = false;
      solver.TimeStepDuration = 1e-4f; <----------------------- changed from 0.01f
      solver.ControlIterationCount = 5000; <-------------------------- changed from 1000
      solver.FixerIterationCount = 5000; <-------------------------- changed from 1000
      solver.VelocitySubiterationCount = 1;
      solver.Solve(controls);

[DJSures]

Answered my own question by adding swing limit as you suggested earlier :) Thank you!

      // Box that rotates around the X axis
      new IKRevoluteJoint(bGround, bBox1, Vector3.Up);
      new IKBallSocketJoint(bGround, bBox1, bBox1.Position - new Vector3(0, bBox1.HalfHeight, 0));

      // Box -> First Arm
      new IKRevoluteJoint(bBox1, bCyl1, Vector3.Forward);
      new IKBallSocketJoint(bBox1, bCyl1, bCyl1.Position - new Vector3(0, bCyl1.HalfHeight, 0));
      new IKSwingLimit(bBox1, bCyl1, Vector3.Up, Vector3.Up, MathHelper.ToRadians(180));

      // First Arm -> Second Arm
      new IKRevoluteJoint(bCyl1, bCyl2, Vector3.Forward);
      new IKBallSocketJoint(bCyl1, bCyl2, bCyl2.Position - new Vector3(0, bCyl2.HalfHeight, 0));
      new IKSwingLimit(bCyl1, bCyl2, Vector3.Up, Vector3.Up, MathHelper.ToRadians(180));

      // Second Arm -> End effector
      new IKRevoluteJoint(bCyl2, bCyl3, Vector3.Forward);
      new IKBallSocketJoint(bCyl2, bCyl3, bCyl3.Position - new Vector3(0, bCyl3.HalfHeight, 0));
      new IKSwingLimit(bCyl2, bCyl3, Vector3.Up, Vector3.Up, MathHelper.ToRadians(90));