Submit My Question - Recent Questions - Recent Answer
Msdn Tech >> Microsoft Robotics Studio >> Microsoft Robotics - Simulation

Moving an "obj" entity periodically via different locations

Let's say my simulation environment contains one "obj" file I imported.

What would be the right way to move it periodically via several locations?

For example:

Start: (0, 0)

MoveTo: (3, 5)

MoveTo: (4, 5)

MoveTo: (4, 3)

End: (0, 0)

Then do it again many times.

Additional two issues are:

1) How to control the entity velocity.

2) How to calculate the correct angle when the entity has to turn (for example if this entity is a human object, his face should be directed toward the next location).

[789 byte] By [UriKartoun] at [2008-1-10]
«« What''s the value of NTDDI_VISTA?
»» Custom Project Template
# 1

So far I did it for one object that moves periodically from one location to another.

Code Snippet
AnimateEntities("street_cone.obj", 4, new Vector3(20, 0, 20));
Code Snippet

public void AnimateEntities(string name, float id, Vector3 pos)
{

Shape cshape = null;

SimplifiedConvexMeshEnvironmentEntity AnimatedEntity = new SimplifiedConvexMeshEnvironmentEntity(
pos,
name,
cshape);

AnimatedEntity.State.MassDensity.Mass = 30;
AnimatedEntity.State.Name = name + id.ToString();

double OriginalX = AnimatedEntity.State.Pose.Position.X;
double OriginalY = AnimatedEntity.State.Pose.Position.Y;

for (int k=1; k<=100; k++)
{
for (double i = AnimatedEntity.State.Pose.Position.X; i <= 30; i = i + 0.05)
{
AnimatedEntity.State.Pose.Position.X = (float)i;
Console.WriteLine(AnimatedEntity.State.Pose.Position.ToString());
System.Threading.Thread.Sleep(20);
SimulationEngine.GlobalInstancePort.Update(AnimatedEntity);
}

for (double j = 30; j >= OriginalX; j = j - 0.05)
{
AnimatedEntity.State.Pose.Position.X = (float)j;
Console.WriteLine(AnimatedEntity.State.Pose.Position.ToString());
System.Threading.Thread.Sleep(20);
SimulationEngine.GlobalInstancePort.Update(AnimatedEntity);
}
}

}

Here is a short video that demonstrates that:
http://www.compactech.com/microsoft/videos/Basic_Entity_Movement_Septmeber_19_2007.wmv

Now it would be better to find a way of not using the "Sleep" method since when there will be many movable entities it will significantly slow down the application.

Maybe I'll try to add a form to my application and use timers. Is it a good idea?

UriKartoun at 2007-10-3 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 2
What about using AnimatedEntity.PhysicsEntity.SetLinearVelocity?
AlexanderDahl at 2007-10-3 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 3

SetLinearVelocity moves the entity to a desired direction over the X, Y and Z axes.

Is there a way to move the entity to a desired position by giving it a velocity parameter?

An example:

The entity is now at (5, 0, 4).

I want to move it to (25, 0, 5) at a velocity of 0.5 meter / second.

How could it be done?

UriKartoun at 2007-10-3 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 4

I think the best way to do this is to override the Update method on your AnimatedEntity. This method is called each frame and one of the parameters it receives is the total time elapsed since the start of the simulation.

I would add a list of waypoints as a member of the entity. A waypoint is a location and a time when you expect the entity to be at that location. Each time Update is called, look at the current time and interpolate between the two waypoints that bracket that time and set the position of the entity accordingly.

As far as changing the orientation of the entity, calculate the angle that the entity needs to face by constructing a vector from its current position to the position of the next waypoint and then normalize it. Find the angle between this vector and the vector (0,0,1) by taking the inverse cosine of the dot product of the two vectors. Set the orientation and position of the entity as:

entity.State.Pose.Orientation = Quaternion.FromAxisAngle(0,1,0,angle); // the way it is facing

entity.State.Pose.Position = <interpolated position>; // where it is

entity.PhysicsEntity.SetPose(entity.State.Pose); // actually change the entity to this pose

which will rotate the entity around its vertical axis by the angle that you specify and set its position properly.

-Kyle

KyleJ-MSFT at 2007-10-3 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 5

I did it differently and it works nicely for one entity, see video:

http://www.compactech.com/microsoft/videos/Uri_Kartoun_Moving_One_Entity_September_25_2007.wmv

I wrote a function that accepts an array of coordinates and sends the entity to a new position based on the difference of both the distance and locations (X and Z) between each two location pairs. This method moves the entity from one location to another in a constanct velocity which is set at the begining of the animation loop.

The code looks like that:

Find the distance between two locations:

Code Snippet
double Entity1Distance = Math.Pow(Math.Pow(EndEntity1.X - StartEntity1.X, 2) + Math.Pow(EndEntity1.Z - StartEntity1.Z, 2), 0.5);

And then there is a loop that moves the entity:

Code Snippet

for (int i = 1; i <= 10 * Entity1Distance; i++)

{


AnimatedEntity1.State.Pose.Position.X = StartEntity1.X + i * (EndEntity1.X - StartEntity1.X) / (10 * (float)Entity1Distance);

AnimatedEntity1.State.Pose.Position.Z = StartEntity1.Z + i * (EndEntity1.Z - StartEntity1.Z) / (10 * (float)Entity1Distance);

SimulationEngine.GlobalInstancePort.Update(AnimatedEntity1);

System.Threading.Thread.Sleep(50);

}

However, I might encounter difficulty if I'll need to animate many entities at the same time. I still have to think about that.

Now I'm struggling to do it the way Kyle has suggested. That's a portion of my code:

BaseTime variable is defined at the *Types.cs file as:

Code Snippet

[DataMember]

public double BaseTime = 0;

And that's the function that is supposed to animate the entity:

Code Snippet

public class MySingleShapeEntity : SingleShapeEntity

{

public MySingleShapeEntity(Shape shape, Vector3 position, Double T1) : base(shape, position, T1) { }

int StartingAnimationFlag = 1;


public override void Update(FrameUpdate update)

{

double BaseTime;


if (StartingAnimationFlag == 1)

{

BaseTime = update.ApplicationTime;

StartingAnimationFlag = 0;

}

// T1: Time at EndPosition

double ElapsedTime = update.ApplicationTime - BaseTime;

Vector3 EndPosition = new Vector3(5f, 0, 0);

Vector3 StartPosition = new Vector3(0, 0, 0);

Vector3 CurrentPosition;

CurrentPosition = (1 - (ElapsedTime - BaseTime) / (T1 - BaseTime)) * StartPosition + (ElapsedTime - BaseTime) / (T1 - BaseTime) * EndPosition;

PhysicsEntity.SetPose(new Pose(CurrentPosition));

base.Update(update);


}

}

The error message I get here is related to the T1 variable (Time at EndPosition); the MySingleShapeEntity function allows me to use only the (Shape shape, Vector3 position) variables and doesn't allow to use an additional variable.

Thanks,

Uri.

UriKartoun at 2007-10-3 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 6

You can't pass T1 to the base constructor because it only expects two parameters.

Here is how I would do it:

Define a new class (AnimatedEntity) which inherits from SingleShapeEntity. Add the _startTime and _waypoints members as shown along with a method to start the animation and a method to set the waypoints. In the update method, calculate an interpolant by determining how long it has been since the animation started and dividing it by the total time of the animation. Use the interpolant to calculate a position for that point in time and use SetPose to move the entity there. If the interpolant is greater than 1, the animation is finished.

I'll leave it to you to enhance this algorithm to handle more than two waypoints.

-Kyle

Code Snippet

private void PopulateWorld()
{
_box1 = AddBox(new Vector3(1, 0.1f, 1));
_box1.State.Name = "box1";
_box1.State.Flags |= EntitySimulationModifiers.Kinematic;
_box2 = AddBox(new Vector3(0, 0.1f, 0));
_box2.State.Name = "box2";
_box2.State.Flags |= EntitySimulationModifiers.Kinematic;
Activate(Arbiter.Receive(false, TimeoutPort(5000), WakeUpBox1));
Activate(Arbiter.Receive(false, TimeoutPort(4000), WakeUpBox2));
}

AnimatedEntity _box1;
AnimatedEntity _box2;
void WakeUpBox1(DateTime now)
{
Waypoint[] waypoints = new Waypoint[]
{
new Waypoint(5f, 0.1f, 0, 0), // start at 5,0.1,0 at 0 seconds
new Waypoint(0, 0.1f, 2, 5) // end at 0,0.1,2 at 5 seconds
};

_box1.SetWaypoints(waypoints);
_box1.StartAnimation();
}
void WakeUpBox2(DateTime now)
{
Waypoint[] waypoints = new Waypoint[]
{
new Waypoint(0, 0.1f, 0, 0), // start at 0,0.1,0 at 0 seconds
new Waypoint(2, 0.1f, 2, 3) // end at 2,0.1,2 at 3 seconds
};

_box2.SetWaypoints(waypoints);
_box2.StartAnimation();
}

public struct Waypoint
{
public float X, Y, Z, T;
public Waypoint(float x, float y, float z, float t)
{
X = x;
Y = y;
Z = z;
T = t;
}
}

public class AnimatedEntity : SingleShapeEntity
{
double _startTime;
bool _animationActive = false;
Waypoint[] _waypoints;

public AnimatedEntity(Shape shape, Vector3 position) : base(shape, position) { }

public void SetWaypoints(Waypoint[] waypoints)
{
_waypoints = waypoints;
}

public void StartAnimation()
{
_startTime = -1; // set this the first time in Update
_animationActive = true;
}

public override void Update(FrameUpdate update)
{
// move the animated entity
if (_animationActive)
{
if (_startTime < 0)
_startTime = update.ApplicationTime;

float i = (float)(update.ApplicationTime - _startTime) / (_waypoints[1].T - _waypoints[0].T);
float oneMinusI = 1f - i;
Vector3 pos = new Vector3();
if (i >= 1)
{
pos.X = _waypoints[1].X;
pos.Y = _waypoints[1].Y;
pos.Z = _waypoints[1].Z;
// finished with animation
_animationActive = false;
}
else
{
pos.X = _waypoints[0].X * oneMinusI + _waypoints[1].X * i;
pos.Y = _waypoints[0].Y * oneMinusI + _waypoints[1].Y * i;
pos.Z = _waypoints[0].Z * oneMinusI + _waypoints[1].Z * i;
}
State.Pose = new Pose(pos);
if (PhysicsEntity != null)
PhysicsEntity.SetPose(State.Pose);
}
base.Update(update);
}
}

KyleJ-MSFT at 2007-10-3 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 7

Kyle, It works! Thank you so much!

Code Snippet

private void PopulateWorld()

{

AddSky();

AddGround();

AddMaze();

// AddRobot();

AddEntities();

_box1 = AddBox(new Vector3(1, 0.1f, 1));

_box1.State.Name = "box1";

_box1.State.Flags |= EntitySimulationModifiers.Kinematic;

_box2 = AddBox(new Vector3(0, 0.1f, 0));

_box2.State.Name = "box2";

_box2.State.Flags |= EntitySimulationModifiers.Kinematic;

Activate(Arbiter.Receive(false, TimeoutPort(5000), WakeUpBox1));

Activate(Arbiter.Receive(false, TimeoutPort(4000), WakeUpBox2));

}

AnimatedEntity _box1;

AnimatedEntity _box2;

private AnimatedEntity AddBox(Vector3 position)

{

Vector3 dimensions =

new Vector3(0.2f, 0.2f, 0.2f); // meters

// create simple movable entity, with a single shape

AnimatedEntity box = new AnimatedEntity(

new BoxShape(

new BoxShapeProperties(

100, // mass in kilograms.

new Pose(), // relative pose

dimensions)), // dimensions

position);

SimulationEngine.GlobalInstancePort.Insert(box);

return box;

}

public void WakeUpBox1(DateTime now)

{

Waypoint[] waypoints = new Waypoint[]

{

new Waypoint(1f, 0.1f, 1f, 0),

new Waypoint(2f, 0.1f, 5f, 3),

new Waypoint(5f, 0.1f, 2f, 5),

new Waypoint(2f, 0.1f, 5f, 7),

new Waypoint(5f, 0.1f, 2f, 9),

};

_box1.SetWaypoints(waypoints);

_box1.StartAnimation();

}

void WakeUpBox2(DateTime now)

{

Waypoint[] waypoints = new Waypoint[]

{

new Waypoint(2f, 0.1f, 1f, 0),

new Waypoint(3f, 0.1f, 8f, 3),

new Waypoint(8f, 0.1f, 4f, 6),

new Waypoint(2f, 0.1f, 1f, 9),

new Waypoint(3f, 0.1f, 7f, 12),

};

_box2.SetWaypoints(waypoints);

_box2.StartAnimation();

}

public struct Waypoint

{

public float X, Y, Z, T;

public Waypoint(float x, float y, float z, float t)

{

X = x;

Y = y;

Z = z;

T = t;

}

}

public class AnimatedEntity : SingleShapeEntity

{

double _startTime;

bool _animationActive = false;

Waypoint[] _waypoints;

int way_point_counter;

public AnimatedEntity(Shape shape, Vector3 position) : base(shape, position) { }

public void SetWaypoints(Waypoint[] waypoints)

{

_waypoints = waypoints;

}

public void StartAnimation()

{

_startTime = -1; // set this the first time in Update

_animationActive = true;

way_point_counter = 0;

}

public override void Update(FrameUpdate update)

{

if (_animationActive)

{

if (_startTime < 0)

_startTime = update.ApplicationTime;

float i = (float)(update.ApplicationTime - (_startTime + _waypoints[way_point_counter].T)) / (_waypoints[way_point_counter + 1].T - _waypoints[way_point_counter].T);

float oneMinusI = 1f - i;

Console.WriteLine(i);

Vector3 pos = new Vector3();

if (i >= 1)

{

pos.X = _waypoints[way_point_counter + 1].X;

pos.Y = _waypoints[way_point_counter + 1].Y;

pos.Z = _waypoints[way_point_counter + 1].Z;

// finished with animation

Console.WriteLine(way_point_counter);

way_point_counter++;

// _animationActive = false;

}

else

{

pos.X = _waypoints[way_point_counter].X * oneMinusI + _waypoints[way_point_counter + 1].X * i;

pos.Y = _waypoints[way_point_counter].Y * oneMinusI + _waypoints[way_point_counter + 1].Y * i;

pos.Z = _waypoints[way_point_counter].Z * oneMinusI + _waypoints[way_point_counter + 1].Z * i;

}

State.Pose = new Pose(pos);

if (PhysicsEntity != null)

{

PhysicsEntity.SetPose(State.Pose);

}

}

base.Update(update);

}

}

Now, the next step - to manipulate an "obj" file and not just a box.

I'm trying to do that by using:

Code Block

Shape cshape = null;

SimplifiedConvexMeshEnvironmentEntity _Entity1 = new SimplifiedConvexMeshEnvironmentEntity(new Vector3 (1, 0.1f,1), "TallBlueGoal.obj", cshape);

_Entity1.State.MassDensity.Mass = 100;

_Entity1.State.Pose.Orientation = Quaternion.FromAxisAngle(0, 1, 0, (float)(45 * Math.PI / 180.0));

SimulationEngine.GlobalInstancePort.Insert(_Entity1);

_Entity1.State.Name = "TallBlueGoal.obj" + "4";

_Entity1.State.Flags |= EntitySimulationModifiers.Kinematic;

Activate(Arbiter.Receive(false, TimeoutPort(5000), WakeUpEntity1));

and:

Code Block

public void WakeUpEntity1(DateTime now)

{

Waypoint[] waypoints = new Waypoint[]

{

new Waypoint(1f, 0.1f, 1f, 0),

new Waypoint(2f, 0.1f, 5f, 3),

new Waypoint(5f, 0.1f, 2f, 5),

new Waypoint(2f, 0.1f, 5f, 7),

new Waypoint(5f, 0.1f, 2f, 9),

};

_Entity1.SetWaypoints(waypoints);

_Entity1.StartAnimation();

}

The "TallBlueGoal.obj4" is inserted correctly but is not animated.

UriKartoun at 2007-10-3 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
Microsoft Robotics Studio
Microsoft Robotics - Simulation

Microsoft Robotics Studio Hot Topic

Microsoft Robotics Studio New Topic

Microsoft Robotics Studio

Java / MSDN / Linux Tech / PHP Tech / MSDN DotNet / MSDN TECH / Health Library / Programmers Tech / visual basic Tech / Developer Tech / Ipod faqs / Msdn China Tech

Site Classified