[SOLVED]AngularMotor Makes Joined Entities Go Crazy

Discuss any questions about BEPUphysics or problems encountered.
Post Reply
squimball
Posts: 7
Joined: Sat May 07, 2011 9:25 pm

[SOLVED]AngularMotor Makes Joined Entities Go Crazy

Post by squimball »

Greetings! I have a little problem that is driving me nuts. Basically, I have 4 objects joined together via ball socket joints. These 4 joints are driven by an angular motor on each one. The shape roughly resembles the ascii-art version below (a base on the bottom, 2 'legs', and a top on the top). The problem is that when I attempt to 'twist' the joints with the angular motors on the Vector3.Up axis (twisting the bottom joints one way and the top joints the opposite direction), the whole model just starts spinning out of control. It seems to work fine at very low twist values (like 1 degree), but the more twist you put on it, the faster it flies out of control.

I cannot for the life of me figure out why it is doing this. I do have the frictions on the models and ground turned down low, so this does sort of amplify the problem. I need low friction though for my particular simulation. I would be very grateful if anyone could either give my an idea why this might be happening. I think I know 'what' is happening, but I don't know 'why' it is happening or how to fix it.

Thanks! :shock:

----
| |
| |
----

Here is an example for what is going on. You should be able to add this to the BEPUPhysicsDemos project if you want to test it out. I was not sure how else to post it here.

AngularMotorDemo.cs

Code: Select all

using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework.Input;
using BEPUphysics.Entities.Prefabs;
using BEPUphysics.Entities;
using BEPUphysics.CollisionRuleManagement;
using BEPUphysics.Constraints;
using BEPUphysics.Constraints.TwoEntity.Joints;
using BEPUphysics.Constraints.TwoEntity.Motors;

using Microsoft.Xna.Framework;

namespace BEPUphysicsDemos.Demos
{
    /// <summary>
    /// Multiple entities connected via ball socket joint controlled by angular motors
    /// </summary>
    public class AngularMotorDemo : StandardDemo
    {
        Entity eGround, eBase, eBox1, eBox2, eTop;
        BallSocketJoint bsBox1Base, bsBox1Top, bsBox2Base, bsBox2Top;
        AngularMotor amBox1Base, amBox1Top, amBox2Base, amBox2Top;

        CollisionGroup bodyGroup = new CollisionGroup();
        List<CollisionGroup> cg = new List<CollisionGroup>();

        float twist = 0f;

        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public AngularMotorDemo(DemosGame game)
            : base(game)
        {
            cg.Add(bodyGroup);
            CollisionGroup.DefineCollisionRulesInSet(cg, CollisionRule.NoBroadPhase, CollisionRule.NoBroadPhase);

            //add some ground
            eGround = new Box(Vector3.Down * 5f, 100f, 10f, 100f);
            eGround.Material.Bounciness = 0f;
            eGround.Material.KineticFriction = 0.25f;
            eGround.Material.StaticFriction = 0.25f;            
            Space.Add(eGround);

            //add the base ent
            eBase = new Box(Vector3.Up * 0.5f, 10f, 1f, 10f, 1f);
            eBase.Material.Bounciness = 0f;
            eBase.Material.KineticFriction = 0.25f;
            eBase.Material.StaticFriction = 0.25f;
            eBase.CollisionInformation.CollisionRules.Group = bodyGroup;
            Space.Add(eBase);

            //add the 1st box ent
            eBox1 = new Box(Vector3.Up * 5f + Vector3.Left * 3f, 2f, 8f, 2f, 1f);
            eBox1.Material.Bounciness = 0f;
            eBox1.Material.KineticFriction = 0.25f;
            eBox1.Material.StaticFriction = 0.25f;
            eBox1.CollisionInformation.CollisionRules.Group = bodyGroup;
            Space.Add(eBox1);

            //add the 2st box ent
            eBox2 = new Box(Vector3.Up * 5f + Vector3.Right * 3f, 2f, 8f, 2f, 1f);
            eBox2.Material.Bounciness = 0f;
            eBox2.Material.KineticFriction = 0.25f;
            eBox2.Material.StaticFriction = 0.25f;
            eBox2.CollisionInformation.CollisionRules.Group = bodyGroup;
            Space.Add(eBox2);

            //add the top ent
            eTop = new Box(Vector3.Up * 10f, 8f, 2f, 2f, 1f);
            eTop.Material.Bounciness = 0f;
            eTop.Material.KineticFriction = 0.25f;
            eTop.Material.StaticFriction = 0.25f;
            eTop.CollisionInformation.CollisionRules.Group = bodyGroup;
            Space.Add(eTop);

            //add the joints connecting it all together
            bsBox1Base = new BallSocketJoint(eBox1, eBase, eBox1.Position + Vector3.Down * 4f);
            bsBox1Top = new BallSocketJoint(eBox1, eTop, eBox1.Position + Vector3.Up * 4f);
            bsBox2Base = new BallSocketJoint(eBox2, eBase, eBox2.Position + Vector3.Down * 4f);
            bsBox2Top = new BallSocketJoint(eBox2, eTop, eBox2.Position + Vector3.Up * 4f);

            //add the angular motors to the joints
            amBox1Base = new AngularMotor(eBox1, eBase);
            amBox1Base.Settings.Mode = MotorMode.Servomechanism;
            amBox1Top = new AngularMotor(eBox1, eTop);
            amBox1Top.Settings.Mode = MotorMode.Servomechanism;
            amBox2Base = new AngularMotor(eBox2, eBase);
            amBox2Base.Settings.Mode = MotorMode.Servomechanism;
            amBox2Top = new AngularMotor(eBox2, eTop);
            amBox2Top.Settings.Mode = MotorMode.Servomechanism;

            Space.Add(bsBox1Base);
            Space.Add(amBox1Base);
            Space.Add(bsBox1Top);
            Space.Add(amBox1Top);
            Space.Add(bsBox2Base);
            Space.Add(amBox2Base);
            Space.Add(bsBox2Top);
            Space.Add(amBox2Top);

            game.Camera.Position = new Vector3(0f, 10f, 30f);
            game.Camera.Pitch = MathHelper.ToRadians(-10f);

        }

        public override void Update(float dt)
        {
            if (Game.KeyboardInput.IsKeyDown(Keys.Left))
                twist += -0.1f * dt;

            if (Game.KeyboardInput.IsKeyDown(Keys.Right))
                twist += 0.1f * dt;

            twist = MathHelper.Clamp(twist, MathHelper.ToRadians(-15f), MathHelper.ToRadians(15f));

            Quaternion q;

            q = Quaternion.Identity;
            q *= Quaternion.CreateFromAxisAngle(Vector3.Up, twist);
            amBox1Base.Settings.Servo.Goal = q;
            amBox2Base.Settings.Servo.Goal = q;

            q = Quaternion.Identity;
            q *= Quaternion.CreateFromAxisAngle(Vector3.Up, -twist);
            amBox1Top.Settings.Servo.Goal = q;
            amBox2Top.Settings.Servo.Goal = q;

            base.Update(dt);
        }

        public override void DrawUI()
        {
            base.DrawUI();
            Game.DataTextDrawer.Draw("Twist Controls:", new Vector2(50, 20));
            Game.TinyTextDrawer.Draw("Left and Right Arrow Keys", new Vector2(60, 40));
            Game.DataTextDrawer.Draw("Current Twist Angle: " + MathHelper.ToDegrees(twist).ToString("0.0") + " degrees", new Vector2(50, 70));
        }

        /// <summary>
        /// Gets the name of the simulation.
        /// </summary>
        public override string Name
        {
            get { return "Angular Motor Test"; }
        }
    }
}
Last edited by squimball on Thu Feb 23, 2012 11:02 pm, edited 1 time in total.
squimball
Posts: 7
Joined: Sat May 07, 2011 9:25 pm

Re: AngularMotor Makes Joined Entities Go Crazy

Post by squimball »

I should mention... By 'driven by', I mean 'their angular orientation is set by'. I'm not trying to spin these like a wheel on a car - just setting their orientations.

Also, what I 'think' the main problem is... I think the way I have the entities connected, when I twist them they are getting 'pulled' to the side and the angles between them are no longer perpendicular. I'm guessing this messes up the original angular motor axes but I do not know how to make them compensate for the altered angle. In other words, I am twisting the angular motors on one axis, but the attached entities are getting 'twisted' on multiple axes and they do not match up anymore.. :shock:
Norbo
Site Admin
Posts: 4929
Joined: Tue Jul 04, 2006 4:45 am

Re: AngularMotor Makes Joined Entities Go Crazy

Post by Norbo »

This type of behavior is usually caused by fighting constraints. In this case, every constraint is extremely strong/rigid and every constraint is fighting every other constraint. This is very hard to solve precisely. Given sufficient solving time, the simulation would eventually calm down- somewhere between 100-500 solver iterations seems to do the trick (the default Space.Solver.IterationLimit is 10, and most things early-out before that point even).

A much more practical solution is to weaken the constraints so they aren't quite so rigid, and don't require immense forces to deform. Dividing their spring constants by 100 seems to be about right:

Code: Select all

            bsBox1Base.SpringSettings.DampingConstant /= 100;
            bsBox1Base.SpringSettings.StiffnessConstant /= 100;
            bsBox1Top.SpringSettings.DampingConstant /= 100;
            bsBox1Top.SpringSettings.StiffnessConstant /= 100;
            bsBox2Base.SpringSettings.DampingConstant /= 100;
            bsBox2Base.SpringSettings.StiffnessConstant /= 100;
            bsBox2Top.SpringSettings.DampingConstant /= 100;
            bsBox2Top.SpringSettings.StiffnessConstant /= 100;

            amBox1Base.Settings.Servo.SpringSettings.DampingConstant /= 100;
            amBox1Base.Settings.Servo.SpringSettings.StiffnessConstant /= 100;
            amBox1Top.Settings.Servo.SpringSettings.DampingConstant /= 100;
            amBox1Top.Settings.Servo.SpringSettings.StiffnessConstant /= 100;
            amBox2Base.Settings.Servo.SpringSettings.DampingConstant /= 100;
            amBox2Base.Settings.Servo.SpringSettings.StiffnessConstant /= 100;
            amBox2Top.Settings.Servo.SpringSettings.DampingConstant /= 100;
            amBox2Top.Settings.Servo.SpringSettings.StiffnessConstant /= 100;
With this configuration, no increase in solver iterations is required and it seems to run robustly.

Thanks for the demo by the way, it makes diagnosing things a lot easier! :)
squimball
Posts: 7
Joined: Sat May 07, 2011 9:25 pm

Re: AngularMotor Makes Joined Entities Go Crazy

Post by squimball »

^ EXCELLENT!! Works like a charm. Thanks!

Here is a modified demo that includes manipulation on all 3 axes of all the joints if anyone wants it. I just wanted to test on multiple axes to make sure it would work. Sure enough! :mrgreen:

AngularMotorDemo.cs

Code: Select all

using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework.Input;
using BEPUphysics.Entities.Prefabs;
using BEPUphysics.Entities;
using BEPUphysics.CollisionRuleManagement;
using BEPUphysics.Constraints;
using BEPUphysics.Constraints.TwoEntity.Joints;
using BEPUphysics.Constraints.TwoEntity.Motors;

using Microsoft.Xna.Framework;

namespace BEPUphysicsDemos.Demos
{
    /// <summary>
    /// Multiple entities connected via ball socket joint controlled by angular motors
    /// </summary>
    public class AngularMotorDemo : StandardDemo
    {
        //entities for the physical entities
        Entity eGround, eBase, eBox1, eBox2, eTop;

        //joints to connect the physical entities together
        BallSocketJoint bsBox1Base, bsBox1Top, bsBox2Base, bsBox2Top;

        //motor to set the orientaions/positions of the physical entities
        AngularMotor amBox1Base, amBox1Top, amBox2Base, amBox2Top;

        //collision group stuff to keep our connected entities from colliding with each other
        CollisionGroup bodyGroup = new CollisionGroup();
        List<CollisionGroup> cg = new List<CollisionGroup>();

        //variables to hold the angular motor angles
        float twist = 0f;
        float bend = 0f;
        float lean = 0f;

        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public AngularMotorDemo(DemosGame game)
            : base(game)
        {
            //define our collision rule for the connected entities
            cg.Add(bodyGroup);
            CollisionGroup.DefineCollisionRulesInSet(cg, CollisionRule.NoBroadPhase, CollisionRule.NoBroadPhase);

            //add some ground
            eGround = new Box(Vector3.Down * 5f, 100f, 10f, 100f);
            eGround.Material.Bounciness = 0f;
            eGround.Material.KineticFriction = 0.25f;
            eGround.Material.StaticFriction = 0.25f;            
            Space.Add(eGround);

            //add the base entity
            eBase = new Box(Vector3.Up * 0.5f, 10f, 1f, 10f, 5f);
            eBase.Material.Bounciness = 0f;
            eBase.Material.KineticFriction = 0.25f;
            eBase.Material.StaticFriction = 0.25f;
            eBase.CollisionInformation.CollisionRules.Group = bodyGroup;
            Space.Add(eBase);

            //add the 1st box entity
            eBox1 = new Box(Vector3.Up * 5f + Vector3.Left * 3f, 2f, 8f, 2f, 1f);
            eBox1.Material.Bounciness = 0f;
            eBox1.Material.KineticFriction = 0.25f;
            eBox1.Material.StaticFriction = 0.25f;
            eBox1.CollisionInformation.CollisionRules.Group = bodyGroup;
            Space.Add(eBox1);

            //add the 2st box entity
            eBox2 = new Box(Vector3.Up * 5f + Vector3.Right * 3f, 2f, 8f, 2f, 1f);
            eBox2.Material.Bounciness = 0f;
            eBox2.Material.KineticFriction = 0.25f;
            eBox2.Material.StaticFriction = 0.25f;
            eBox2.CollisionInformation.CollisionRules.Group = bodyGroup;
            Space.Add(eBox2);

            //add the top entity
            eTop = new Box(Vector3.Up * 13f, 8f, 8f, 2f, 1f);
            eTop.Material.Bounciness = 0f;
            eTop.Material.KineticFriction = 0.25f;
            eTop.Material.StaticFriction = 0.25f;
            eTop.CollisionInformation.CollisionRules.Group = bodyGroup;
            Space.Add(eTop);

            //add the joints connecting it all together
            bsBox1Base = new BallSocketJoint(eBox1, eBase, eBox1.Position + Vector3.Down * 4f);
            bsBox1Top = new BallSocketJoint(eBox1, eTop, eBox1.Position + Vector3.Up * 4f);
            bsBox2Base = new BallSocketJoint(eBox2, eBase, eBox2.Position + Vector3.Down * 4f);
            bsBox2Top = new BallSocketJoint(eBox2, eTop, eBox2.Position + Vector3.Up * 4f);

            //add the angular motors between each connected entity pair
            amBox1Base = new AngularMotor(eBox1, eBase);            
            amBox1Top = new AngularMotor(eBox1, eTop);            
            amBox2Base = new AngularMotor(eBox2, eBase);            
            amBox2Top = new AngularMotor(eBox2, eTop);

            //change the angular motors to servo mode
            amBox1Base.Settings.Mode = MotorMode.Servomechanism;
            amBox1Top.Settings.Mode = MotorMode.Servomechanism;
            amBox2Base.Settings.Mode = MotorMode.Servomechanism;
            amBox2Top.Settings.Mode = MotorMode.Servomechanism;

            //weaken the contraints to prevent mayhem - THANKS NORBO!!!  :D
            bsBox1Base.SpringSettings.DampingConstant /= 100;
            bsBox1Base.SpringSettings.StiffnessConstant /= 100;
            bsBox1Top.SpringSettings.DampingConstant /= 100;
            bsBox1Top.SpringSettings.StiffnessConstant /= 100;
            bsBox2Base.SpringSettings.DampingConstant /= 100;
            bsBox2Base.SpringSettings.StiffnessConstant /= 100;
            bsBox2Top.SpringSettings.DampingConstant /= 100;
            bsBox2Top.SpringSettings.StiffnessConstant /= 100;

            amBox1Base.Settings.Servo.SpringSettings.DampingConstant /= 100;
            amBox1Base.Settings.Servo.SpringSettings.StiffnessConstant /= 100;            
            amBox1Top.Settings.Servo.SpringSettings.DampingConstant /= 100;
            amBox1Top.Settings.Servo.SpringSettings.StiffnessConstant /= 100;            
            amBox2Base.Settings.Servo.SpringSettings.DampingConstant /= 100;
            amBox2Base.Settings.Servo.SpringSettings.StiffnessConstant /= 100;            
            amBox2Top.Settings.Servo.SpringSettings.DampingConstant /= 100;
            amBox2Top.Settings.Servo.SpringSettings.StiffnessConstant /= 100;

            //add the joints and motors to space
            Space.Add(bsBox1Base);
            Space.Add(amBox1Base);
            Space.Add(bsBox1Top);
            Space.Add(amBox1Top);
            Space.Add(bsBox2Base);
            Space.Add(amBox2Base);
            Space.Add(bsBox2Top);
            Space.Add(amBox2Top);

            //set starting camera position/orientation
            game.Camera.Position = new Vector3(0f, 13f, 30f);
            game.Camera.Pitch = MathHelper.ToRadians(-10f);

            //set gravity
            Space.ForceUpdater.Gravity = new Vector3(0f, -98.6f, 0f);
        }

        public override void Update(float dt)
        {
            //twist the joints
            if (Game.KeyboardInput.IsKeyDown(Keys.Left))
                twist += -1f * dt;

            if (Game.KeyboardInput.IsKeyDown(Keys.Right))
                twist += 1f * dt;

            //bend the joints
            if (Game.KeyboardInput.IsKeyDown(Keys.Up))
                bend += -1f * dt;

            if (Game.KeyboardInput.IsKeyDown(Keys.Down))
                bend += 1f * dt;

            //lean the joints
            if (Game.KeyboardInput.IsKeyDown(Keys.Delete))
                lean += 1f * dt;

            if (Game.KeyboardInput.IsKeyDown(Keys.PageDown))
                lean += -1f * dt;

            //limit the orientation variables to reasonable ranges
            twist = MathHelper.Clamp(twist, MathHelper.ToRadians(-60f), MathHelper.ToRadians(60f));
            bend = MathHelper.Clamp(bend, MathHelper.ToRadians(-60f), MathHelper.ToRadians(60f));
            lean = MathHelper.Clamp(lean, MathHelper.ToRadians(-60f), MathHelper.ToRadians(60f));

            Quaternion q;

            //calculate base joint orientations and set them
            q = Quaternion.Identity;
            q *= Quaternion.CreateFromAxisAngle(Vector3.Forward, lean);
            q *= Quaternion.CreateFromAxisAngle(Vector3.Right, bend);
            q *= Quaternion.CreateFromAxisAngle(Vector3.Up, twist);
            amBox1Base.Settings.Servo.Goal = q;
            amBox2Base.Settings.Servo.Goal = q;

            //calculate top joint orientations and set them (invert the twist)
            q = Quaternion.Identity;
            q *= Quaternion.CreateFromAxisAngle(Vector3.Forward, lean);
            q *= Quaternion.CreateFromAxisAngle(Vector3.Right, bend);
            q *= Quaternion.CreateFromAxisAngle(Vector3.Up, -twist);
            amBox1Top.Settings.Servo.Goal = q;
            amBox2Top.Settings.Servo.Goal = q;

            base.Update(dt);
        }

        public override void DrawUI()
        {
            base.DrawUI();

            //draw controls and current angles
            Game.DataTextDrawer.Draw("Twist Controls:", new Vector2(50, 20));
            Game.TinyTextDrawer.Draw("Left and Right Arrow Keys", new Vector2(60, 40));
            Game.DataTextDrawer.Draw("Bend Controls:", new Vector2(50, 60));
            Game.TinyTextDrawer.Draw("Up and Down Arrow Keys", new Vector2(60, 80));
            Game.DataTextDrawer.Draw("Lean Controls:", new Vector2(50, 100));
            Game.TinyTextDrawer.Draw("Delete and Page Down Keys", new Vector2(60, 120));
            Game.DataTextDrawer.Draw("Current Twist Angle: " + MathHelper.ToDegrees(twist).ToString("0.0") + " degrees", new Vector2(50, 150));
            Game.DataTextDrawer.Draw("Current Bend Angle: " + MathHelper.ToDegrees(bend).ToString("0.0") + " degrees", new Vector2(50, 180));
            Game.DataTextDrawer.Draw("Current Lean Angle: " + MathHelper.ToDegrees(lean).ToString("0.0") + " degrees", new Vector2(50, 210));
        }

        /// <summary>
        /// Gets the name of the simulation.
        /// </summary>
        public override string Name
        {
            get { return "Angular Motor Test"; }
        }
    }
}
Post Reply