public override void UpdateEntity(int entity)
{
CPosition position = (CPosition)positions[entity];
CPredator predator = (CPredator)predators[entity];
CPhysicsBody physicsBody = (CPhysicsBody)physicsBodies[entity];
CTransform transform = (CTransform)transforms[entity];
CBoid boid = (CBoid)boids[entity];
if (!predator.Seeking)
{
predator.Seeking = true;
predator.Target = new Vector2(Morro.Maths.Random.Range(0, (int)scene.SceneBounds.Width), Morro.Maths.Random.Range(0, (int)scene.SceneBounds.Height));
}
else
{
Vector2 positionVector = new Vector2(position.X, position.Y);
if (Vector2.Distance(positionVector, predator.Target) < 8)
{
predator.Seeking = false;
return;
}
Vector2 desired = predator.Target - positionVector;
desired.SetMagnitude(boid.MoveSpeed);
Vector2 seek = desired - physicsBody.Velocity;
seek.Limit(boid.MaxForce);
physicsBody.Velocity += seek;
transform.Rotation = -(float)Math.Atan2(physicsBody.Velocity.Y, physicsBody.Velocity.X);
}
}
public VertexTransform(CPosition position, CDimension dimension, CTransform transform)
{
Scale = new Vector3(dimension.Width * transform.Scale.X, dimension.Height * transform.Scale.Y, transform.Scale.Z);
RotationOffset = new Vector3(transform.RotationOffset.X, transform.RotationOffset.Y, 0);
Rotation = transform.Rotation;
Translation = new Vector3(position.X + transform.Translation.X, position.Y + transform.Translation.Y, position.Z + transform.Translation.Z);
}
private static void VelocityVerletIntegration(float deltaTime, CPosition position, CPhysicsBody physicsBody)
{
position.X += physicsBody.Velocity.X * deltaTime + 0.5f * physicsBody.Acceleration.X * deltaTime * deltaTime;
position.Y += physicsBody.Velocity.Y * deltaTime + 0.5f * physicsBody.Acceleration.Y * deltaTime * deltaTime;
physicsBody.Velocity += physicsBody.Acceleration * deltaTime;
}
public int GetGridPosFromMapPos(double x, double y, out CPosition <int> gridPos)
{
return(GetGridPosFromMapPos(new CPosition <double>()
{
x = x, y = y
}, out gridPos));
}
private void SemiImplictEulerIntegration(float deltaTime, CPosition position, CPhysicsBody physicsBody)
{
physicsBody.Velocity += physicsBody.Acceleration * deltaTime;
position.X += physicsBody.Velocity.X * deltaTime;
position.Y += physicsBody.Velocity.Y * deltaTime;
}
public static IComponent[] Create(float x, float y)
{
CBoid boid = new CBoid()
{
ViewRadius = 16,
MoveSpeed = Morro.Maths.Random.Range(60, 80),
MaxForce = 0.4f
};
CPhysicsBody physicsBody = new CPhysicsBody(Morro.Maths.Random.RandomVector2(boid.MoveSpeed), Microsoft.Xna.Framework.Vector2.Zero);
CPosition position = new CPosition(x, y);
CDimension dimension = new CDimension(4, 4);
CTransform transform = new CTransform()
{
Scale = new Microsoft.Xna.Framework.Vector3(3, 1, 1),
Rotation = -(float)Math.Atan2(physicsBody.Velocity.Y, physicsBody.Velocity.X),
RotationOffset = new Microsoft.Xna.Framework.Vector2(dimension.Width / 2, dimension.Height / 2)
};
CColor color = new CColor(PICO8.MidnightBlack);
return(new IComponent[]
{
boid,
physicsBody,
position,
dimension,
transform,
color,
new CTriangle(),
new CPartitionable(),
new CPredator()
});
}
/// <summary>
/// 地图位置转格子位置 返回格子下标
/// </summary>
/// <param name="mapPos"></param>
/// <param name="gridPos"></param>
/// <return></return>
public int GetGridPosFromMapPos(CPosition <double> mapPos, out CPosition <int> gridPos)
{
gridPos = new CPosition <int> {
x = (int)(mapPos.x - _map.MapMinX) / _grid.GridUnit, y = (int)(mapPos.y - _map.MapMinY) / _grid.GridUnit
};
return(GetGridIdxFromGridPos(gridPos));
}
public override void UpdateEntity(int entity)
{
CPosition position = (CPosition)positions[entity];
if (position.Y > scene.SceneBounds.Bottom + 64 || position.X < scene.SceneBounds.Left - 64 || position.X > scene.SceneBounds.Right + 64)
{
scene.RemoveEntity(entity);
}
}
/// <summary>
/// 获得两个位置的相对格子距离
/// </summary>
/// <param name="aPos"></param>
/// <param name="bPos"></param>
/// <returns></returns>
public int GetDistanceFromTwoPosition(CPosition <double> aPos, CPosition <double> bPos)
{
GetGridPosFromMapPos(aPos, out var aGridPos);
GetGridPosFromMapPos(bPos, out var bGridPos);
var res = System.Math.Max(System.Math.Abs(aGridPos.x - bGridPos.x), System.Math.Abs(aGridPos.y - bGridPos.y));
return(res);
}
public ERole() : base()
{
EntityType = EEntityType.ROLE;
Position = new CPosition <double> {
x = 1D, y = 1D, z = 0D
};
SightDistance = 150;
CanBeSeeDistance = 150;
AutoAttackDistance = 10;
LastActiveTime = Program.stopwatch.ElapsedMilliseconds;
}
private void Integrate(CPosition position, CPhysicsBody physicsBody, Integrator integrator, float deltaTime)
{
switch (integrator)
{
case Integrator.SemiImplictEuler:
SemiImplictEulerIntegration(deltaTime, position, physicsBody);
break;
case Integrator.VelocityVerlet:
VelocityVerletIntegration(deltaTime, position, physicsBody);
break;
}
}
internal static SchemaShape2D CreateShapeSchema(GeometryData shapeData, CPosition position, CDimension dimension, CTransform transform)
{
Matrix vertexTransform = CreateVertexTransform();
Vector2[] vertices = CreateVertices();
LineSegment[] lineSegments = CreateLineSegments();
return(new SchemaShape2D(vertices, lineSegments));
Matrix CreateVertexTransform()
{
return
(Matrix.CreateScale(dimension.Width * transform.Scale.X, dimension.Height * transform.Scale.Y, 1 * transform.Scale.Z) *
Matrix.CreateTranslation(-new Vector3(transform.RotationOffset.X, transform.RotationOffset.Y, 0)) *
Matrix.CreateRotationZ(transform.Rotation) *
Matrix.CreateTranslation(position.X + transform.Translation.X + transform.RotationOffset.X, position.Y + transform.Translation.Y + transform.RotationOffset.Y, transform.Translation.Z) *
Matrix.Identity);
}
Vector2[] CreateVertices()
{
Vector2[] result = new Vector2[shapeData.TotalVertices];
for (int i = 0; i < shapeData.TotalVertices; i++)
{
result[i] = Vector2.Transform(new Vector2(shapeData.Vertices[i].X, shapeData.Vertices[i].Y), vertexTransform);
}
return(result);
}
LineSegment[] CreateLineSegments()
{
int totalVertices = shapeData.TotalVertices;
LineSegment[] result = new LineSegment[totalVertices];
result[0] = new LineSegment(vertices[totalVertices - 1].X, vertices[totalVertices - 1].Y, vertices[0].X, vertices[0].Y);
for (int i = 1; i < totalVertices; i++)
{
result[i] = new LineSegment(vertices[i - 1].X, vertices[i - 1].Y, vertices[i].X, vertices[i].Y);
}
return(result);
}
}
private void Simultate(int entity)
{
CPosition position = (CPosition)positions[entity];
CPhysicsBody physicsBody = (CPhysicsBody)physicsBodies[entity];
physicsBody.Accumulator += (float)(Engine.TotalGameTime - physicsBody.LastUpdate).TotalSeconds;
physicsBody.LastUpdate = new TimeSpan(Engine.TotalGameTime.Ticks);
while (physicsBody.Accumulator >= target)
{
Integrate(position, physicsBody, integrator, target);
physicsBody.Accumulator -= target;
}
}
/// <summary>
/// 更新Entity位置
/// </summary>
/// <param name="self"></param>
/// <param name="x"></param>
/// <param name="y"></param>
public void UpdateEntityPosition(EEntity self, double x, double y, bool isFirst = false)
{
// 位置无效
if (!IsValidPos(x, y))
{
return;
}
var oldPosition = new CPosition <double> {
x = self.Position.x, y = self.Position.y
};
// 格子位置不变时说明AOI没变化 所以不必通知AOI内角色
var oldGridId = GetGridPosFromMapPos(oldPosition, out var _);
var newGridId = GetGridPosFromMapPos(x, y, out var _);
if (oldGridId == newGridId && !isFirst)
{
self.Position.x = x;
self.Position.y = y;
return;
}
GetEntitiesFromSight(self, oldPosition, out _, out var oldEntities);
// 从旧格子中删除
DeleteEntityFromGrid(self);
// 更新位置
self.Position.x = x;
self.Position.y = y;
// 加入新格子
AddEntityToGrid(self);
GetEntitiesFromSight(self, self.Position, out _, out var newEntities);
if (!isFirst)
{// 首次进入AOI不需要排除
var bothTmp = new HashSet <EEntity>(oldEntities);
bothTmp.IntersectWith(newEntities);
oldEntities.ExceptWith(bothTmp);
newEntities.ExceptWith(bothTmp);
}
oldEntities.Remove(self);
SSight.Instance.LeaveSight(self, oldEntities);
SSight.Instance.EnterSight(self, newEntities);
}
private void ProcessPolygonObject(Quadtree <GameObject> hash, PolygonObject obj,
Action <GameObject, BaseObject> func)
{
var entity = new GameObject();
var bounds = new CCollisionBound(obj.Polygon.Select(point => new Vector2(point.X, point.Y)).ToList());
var position = new CPosition(new Vector2(obj.X, obj.Y));
entity.Components.Add(bounds);
entity.Components.Add(position);
func(entity, obj);
hash.Add(entity, new RectangleF(
position.Position.X,
position.Position.Y,
bounds.Bounds.Width,
bounds.Bounds.Height
));
}
private void ProcessRectangleObject(Quadtree <GameObject> hash, RectangleObject obj,
Action <GameObject, BaseObject> func)
{
var entity = new GameObject();
var bounds = CCollisionBound.Rectangle(new Vector2(obj.Width, obj.Height));
var position = new CPosition(new Vector2(obj.X, obj.Y));
entity.Components.Add(bounds);
entity.Components.Add(position);
func(entity, obj);
hash.Add(entity, new RectangleF(
position.Position.X,
position.Position.Y,
bounds.Bounds.Width,
bounds.Bounds.Height
));
}
private static (float, float) ProjectPolygon(Vector2 axis, CPosition position, CCollisionBound bounds)
{
var d = Vector2.Dot(position.Position + bounds.Points[0], axis);
var min = d;
var max = d;
foreach (var t in bounds.Points)
{
d = Vector2.Dot(position.Position + t, axis);
if (d < min)
{
min = d;
}
else if (d > max)
{
max = d;
}
}
return(min, max);
}
public static IComponent[] Create(float x, float y, float size)
{
CPosition position = new CPosition(x, y);
CDimension dimension = new CDimension(size, size);
CTransform transform = new CTransform()
{
Rotation = (float)Random.Range(0, System.Math.PI * 2),
RotationOffset = new Vector2(size / 2, size / 2),
};
return(new IComponent[]
{
position,
dimension,
transform,
new CColor(palette[Random.Range(0, palette.Length - 1)]),
new CQuad(),
new CPhysicsBody(Random.RandomVector2(Random.Range(0, 300)), new Vector2(0, 75)),
new CPartitionable()
});
}
/// <summary>
/// 获取周围指定深度的格子下标集合
/// </summary>
/// <param name="deep"></param>
/// <param name="curPos"></param>
/// <param name="gridIdxs"></param>
public void GetRolesFromSight(int deep, CPosition <int> curPos, out HashSet <int> gridIdxs, out HashSet <int> roleIds)
{
gridIdxs = new HashSet <int>();
roleIds = new HashSet <int>();
var xFrom = curPos.x - deep < 0 ? 0 : curPos.x - deep;
var yFrom = curPos.y - deep < 0 ? 0 : curPos.y - deep;
var xTo = curPos.x + deep < _grid.GridWidth ? curPos.x + deep : _grid.GridWidth;
var yTo = curPos.y + deep < _grid.GridLength ? curPos.y + deep : _grid.GridLength;
for (var i = xFrom; i <= xTo; ++i)
{
for (var j = yFrom; j <= yTo; ++j)
{
if (i >= 0 && i < _grid.GridWidth && j >= 0 && j < _grid.GridLength)
{
var idx = GetGridIdxFromGridPos(i, j);
gridIdxs.Add(idx);
roleIds.UnionWith(_entityIdMap[idx][(int)EEntityType.ROLE]);
}
}
}
}
public override void UpdateEntity(int entity)
{
CPosition position = (CPosition)positions[entity];
CDimension dimension = (CDimension)dimensions[entity];
if (position.X + dimension.Width < scene.SceneBounds.Left)
{
position.X = scene.SceneBounds.Right;
}
else if (position.X > scene.SceneBounds.Right)
{
position.X = scene.SceneBounds.Left - dimension.Width;
}
if (position.Y + dimension.Height < scene.SceneBounds.Top)
{
position.Y = scene.SceneBounds.Bottom;
}
else if (position.Y > scene.SceneBounds.Bottom)
{
position.Y = scene.SceneBounds.Top - dimension.Height;
}
}
private void DoWork()
{
while (m_Run)
{
Gtk.Application.Invoke (delegate {
try
{
position = jaco.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
}
catch(Exception)
{
System.Console.WriteLine("EXCEPTION");
}
});
m_OutTimeAbsolute.Text = position.TimeAbsolute.ToString();
m_OutTimeStartUp.Text = position.TimeFromStartup.ToString();
m_OutTimeStampSavings.Text = position.TimeStampSavings.ToString();
m_OutSupplyVoltage.Text = position.SupplyVoltage.ToString();
m_OutCurrentConsumed.Text = position.CurrentConsumed.ToString();
m_OutPowerConsumed.Text = position.PowerConsumed.ToString();
m_OutAveragePower.Text = position.AveragePower.ToString();
m_OutAccelerationX.Text = position.AccelerationCaptorX.ToString();
m_OutAccelerationY.Text = position.AccelerationCaptorY.ToString();
m_OutAccelerationZ.Text = position.AccelerationCaptorZ.ToString();
m_OutCodeVersion.Text = position.CodeVersion.ToString("X1");
m_OutCodeRevision.Text = position.CodeRevision.ToString("X1");
m_OutControlOperator.Text = position.ControlOperator.ToString();
m_OutControlMode.Text = position.ControlMode.ToString();
m_OutHandMode.Text = position.HandMode.ToString();
m_OutConnectedJointQuantity.Text = position.ConnectedJointQuantity.ToString();
m_OutPositionType.Text = position.PositionType.ToString();
m_OutErrorsSpiMain.Text = position.NbErrorsSpiPrincipal.ToString();
m_OutErrorsSpiExternal.Text = position.NbErrorsSpiExternal.ToString();
m_OutErrorsMainCAN.Text = position.NbErrorsCanPrincipal.ToString();
m_OutErrorsExternalCAN.Text = position.NbErrorsCanExternal.ToString();
m_OutCartesianX.Text = position.UserPosition.Position.Position[0].ToString();
m_OutCartesianY.Text = position.UserPosition.Position.Position[1].ToString();
m_OutCartesianZ.Text = position.UserPosition.Position.Position[2].ToString();
m_OutCartesianThetaX.Text = position.UserPosition.Position.Rotation[0].ToString();
m_OutCartesianThetaY.Text = position.UserPosition.Position.Rotation[1].ToString();
m_OutCartesianThetaZ.Text = position.UserPosition.Position.Rotation[2].ToString();
m_OutAngle1.Text = position.UserPosition.AnglesJoints.Angle[0].ToString();
m_OutAngle2.Text = position.UserPosition.AnglesJoints.Angle[1].ToString();
m_OutAngle3.Text = position.UserPosition.AnglesJoints.Angle[2].ToString();
m_OutAngle4.Text = position.UserPosition.AnglesJoints.Angle[3].ToString();
m_OutAngle5.Text = position.UserPosition.AnglesJoints.Angle[4].ToString();
m_OutAngle6.Text = position.UserPosition.AnglesJoints.Angle[5].ToString();
Thread.Sleep(100);
}
}
public PlayerData()
{
Position = new CPosition(Vector2.Zero);
Flags = new Dictionary <string, bool>();
Variables = new Dictionary <string, IComparable>();
}
public override void UpdateEntity(int entity)
{
CBoid boid = (CBoid)boids[entity];
CPosition position = (CPosition)positions[entity];
CDimension dimension = (CDimension)dimensions[entity];
CTransform transform = (CTransform)transforms[entity];
CPhysicsBody physicsBody = (CPhysicsBody)physicsBodies[entity];
Vector2 myCenter = new Vector2(position.X + dimension.Width / 2, position.Y + dimension.Height / 2);
Vector2 cumulativeSeperation = Vector2.Zero;
int totalSeperation = 0;
Vector2 cumulativeAlignment = Vector2.Zero;
int totalAlignment = 0;
Vector2 cumulativeCohesion = Vector2.Zero;
int totalCohesion = 0;
float distance;
CPosition theirPosition;
CDimension theirDimension;
CPhysicsBody theirPhysicsBody;
Vector2 force;
Vector2 theirCenter;
List <int> queryResult = binPartitioner.Query(new Morro.Core.Rectangle(position.X - boid.ViewRadius, position.Y - boid.ViewRadius, dimension.Width + boid.ViewRadius * 2, dimension.Height + boid.ViewRadius * 2));
for (int i = 0; i < queryResult.Count; i++)
{
if (queryResult[i] == entity)
{
continue;
}
theirPosition = (CPosition)positions[queryResult[i]];
theirDimension = (CDimension)dimensions[queryResult[i]];
theirPhysicsBody = (CPhysicsBody)physicsBodies[queryResult[i]];
theirCenter = new Vector2(theirPosition.X + theirDimension.Width / 2, theirPosition.Y + theirDimension.Height / 2);
distance = Vector2.Distance(myCenter, theirCenter);
if (distance > 0)
{
if (distance < boid.ViewRadius)
{
cumulativeCohesion += theirCenter;
totalCohesion++;
cumulativeAlignment += theirPhysicsBody.Velocity;
totalAlignment++;
}
if (distance < theirDimension.Width * 2)
{
force = myCenter - theirCenter;
force /= distance * distance;
cumulativeSeperation += force;
totalSeperation++;
}
if (distance < 64 && theirDimension.Width > 2)
{
force = myCenter - theirCenter;
force /= distance * distance;
cumulativeSeperation += force * 4;
totalSeperation++;
}
}
}
distance = Vector2.Distance(Morro.Input.Mouse.SceneLocation, myCenter);
if (distance > 0 && distance < 32)
{
force = myCenter - Morro.Input.Mouse.SceneLocation;
force /= distance * distance;
cumulativeSeperation += force;
totalSeperation++;
}
Vector2 seperation = CalculateSeperation();
Vector2 alignment = CalculateAlignment();
Vector2 cohesion = CalculateCohesion();
Vector2 totalForce = seperation + alignment + cohesion;
physicsBody.Velocity += totalForce;
transform.Rotation = -(float)Math.Atan2(physicsBody.Velocity.Y, physicsBody.Velocity.X);
Vector2 CalculateSeperation()
{
if (totalSeperation <= 0)
{
return(Vector2.Zero);
}
cumulativeSeperation /= totalSeperation;
cumulativeSeperation.SetMagnitude(boid.MoveSpeed);
Vector2 result = cumulativeSeperation - physicsBody.Velocity;
result.Limit(boid.MaxForce);
return(result * seperationIntensity);
}
Vector2 CalculateAlignment()
{
if (totalAlignment <= 0)
{
return(Vector2.Zero);
}
cumulativeAlignment /= totalAlignment;
cumulativeAlignment.SetMagnitude(boid.MoveSpeed);
Vector2 result = cumulativeAlignment - physicsBody.Velocity;
result.Limit(boid.MaxForce);
return(result * alignmentIntensity);
}
Vector2 CalculateCohesion()
{
if (totalCohesion <= 0)
{
return(Vector2.Zero);
}
cumulativeCohesion /= totalCohesion;
cumulativeCohesion -= myCenter;
cumulativeCohesion.SetMagnitude(boid.MoveSpeed);
Vector2 result = cumulativeCohesion - physicsBody.Velocity;
result.Limit(boid.MaxForce);
return(result * cohesionIntensity);
}
}
/// \brief Update the internal state, send commands.
public void Update()
{
CVectorAngle jp = m_Arm.ConfigurationsManager.GetJointPositions();
CVectorEuler hand = m_Arm.ConfigurationsManager.GetHandPosition();
// See the DH specs for angle conversions.
m_State.shoulder_yaw.angle =
(180.0 - jp.Angle[0]) / (180.0 / Math.PI);
m_State.shoulder_pitch.angle =
(jp.Angle[1] - 270.0) / (180.0 / Math.PI);
m_State.elbow_pitch.angle =
(90.0 - jp.Angle[2]) / (180.0 / Math.PI);
m_State.elbow_roll.angle =
(180.0 - jp.Angle[3]) / (180.0 / Math.PI);
m_State.wrist_roll.angle =
(180.0 - jp.Angle[4]) / (180.0 / Math.PI);
m_State.hand_roll.angle =
(260.0 - jp.Angle[5]) / (180.0 / Math.PI);
//Hand pose & orientation
m_State.hand_position_x = hand.Position[0];
m_State.hand_position_y = hand.Position[1];
m_State.hand_position_z = hand.Position[2];
m_State.hand_orientation_x = hand.Rotation[0];
m_State.hand_orientation_y = hand.Rotation[1];
m_State.hand_orientation_z = hand.Rotation[2];
//Update finger information (slow)
float[] fingerPos = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().UserPosition.FingerPosition;
//Update finger angles
m_State.finger_1.angle = fingerPos[0] * Math.PI / 180.0;
m_State.finger_2.angle = fingerPos[1] * Math.PI / 180.0;
m_State.finger_3.angle = fingerPos[2] * Math.PI / 180.0;
//if (m_IsRetracting && ((DateTime.Now - m_LastCmd) > m_RetractDelay))
if(m_IsRetracting)
{
try
{
//Getting position Live Only when retracting and wait for
CPosition positionLive = new CPosition();
positionLive = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
//System.Console.WriteLine("Retract status ");
//System.Console.WriteLine(positionLive.SystemStatus.RetractStatus);
/*
Mode_Normal_To_Ready
Mode_Ready_Standby
Mode_Ready_To_Retract
Mode_Retract_Standby
Mode_Retract_To_Ready
Mode_Normal
Mode_NoInit_To_Ready
ERROR
*/
//Are we done retracting
if (
positionLive.SystemStatus.RetractStatus == 1 ||
positionLive.SystemStatus.RetractStatus == 3)
{
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
else
{
//Look for timeout
if ((DateTime.Now - m_LastCmd) > m_RetractDelay)
{
System.Console.WriteLine("Retract delay expired");
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
}
}
catch (Exception e)
{
System.Console.WriteLine(
"JACO API diagnostic API Failed : ");
System.Console.WriteLine(e.ToString());
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
return;
}
// 200 ms watchdog, reset the command if it's too old.
if ((DateTime.Now - m_LastCmd) > m_WatchDogDelay)
m_Cmd = new CJoystickValue();
// NOTE: Disabled for now, testing trajectory mode.
//m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
}
public void JacoRetract()
{
try
{
System.Console.WriteLine("Jaco arm API Retract()");
m_Cmd = new CJoystickValue();
m_Cmd.ButtonValue[2] = 1;
m_Arm.ControlManager.SendJoystickFunctionality(m_Cmd);
while(m_IsRetracting)
{
try
{
//Getting position Live Only when retracting and wait for
positionLive = new CPosition();
positionLive = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
System.Console.WriteLine("Retract status ");
System.Console.WriteLine(positionLive.SystemStatus.RetractStatus);
//Are we done retracting
if ( positionLive.SystemStatus.RetractStatus == 1 || positionLive.SystemStatus.RetractStatus == 3)
{
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionality(m_Cmd);
m_LastCmd = DateTime.Now;
}
else
{
//Look for timeout
if ((DateTime.Now - m_LastCmd) > m_RetractDelay)
{
System.Console.WriteLine("Retract delay expired");
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionality(m_Cmd);
m_LastCmd = DateTime.Now;
}
}
}
catch (Exception e)
{
System.Console.WriteLine("JACO retract diagnostic API Failed : ");
System.Console.WriteLine(e.ToString());
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionality(m_Cmd);
m_LastCmd = DateTime.Now;
}
}
}
catch (Exception ex)
{
System.Console.WriteLine("EXCEPTION in ");
System.Console.WriteLine(ex.ToString());
}
}
public void JacoRefreshEncoder()
{
try
{
//if (m_Arm.JacoIsReady())
// {
joint_info = m_Arm.ControlManager.GetPositioningAngularInfo();
current_info = m_Arm.ControlManager.GetCurrentAngularInfo();
pose_info = m_Arm.ControlManager.GetCommandCartesianInfo();
trajectory_info = m_Arm.ControlManager.GetInfoFIFOTrajectory();
positionLive = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
//button states
m_State.power_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[0]);
m_State.retract_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[1]);
m_State.one_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[2]);
m_State.two_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[3]);
m_State.three_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[4]);
m_State.left_joystick_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[5]);
m_State.right_joystick_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[6]);
//joystick states
m_State.forward_backward = positionLive.JoystickValue.InclineFB;
m_State.left_right = positionLive.JoystickValue.InclineLR;
m_State.rotate = positionLive.JoystickValue.Rotate;
// Based on Observation of actual model
// getting joints angles
// Normailising the joint angle (-180 to +180)
m_State.shoulder_yaw.angle = Normalize( ((joint_info.Joint1) - 180.0) * Constants.DTR );
m_State.shoulder_pitch.angle = Normalize( ((joint_info.Joint2) - 270.0) * Constants.DTR );
m_State.elbow_pitch.angle = Normalize( ((joint_info.Joint3) - 90.0 ) * Constants.DTR );
m_State.elbow_roll.angle = Normalize( ((joint_info.Joint4) - 180.0) * Constants.DTR );
m_State.wrist_roll.angle = Normalize( ((joint_info.Joint5) - 180.0) * Constants.DTR );
m_State.hand_roll.angle = Normalize( ((joint_info.Joint6) - 260.0) * Constants.DTR );
// based on DH model provided by Kinova
/*m_State.shoulder_yaw.angle = (180.0 - (joint_info.Joint1)) * Constants.DTR;
m_State.shoulder_pitch.angle = ((joint_info.Joint2) - 270.0) * Constants.DTR;
m_State.elbow_pitch.angle = (90.0 - (joint_info.Joint3)) * Constants.DTR;
m_State.elbow_roll.angle = (180.0 - (joint_info.Joint4)) * Constants.DTR;
m_State.wrist_roll.angle = (180.0 - (joint_info.Joint5)) * Constants.DTR;
m_State.hand_roll.angle = (260.0 - (joint_info.Joint6)) * Constants.DTR;*/
// getting joint current
current_info = m_Arm.ControlManager.GetCurrentAngularInfo();
m_State.current_joint_1 = current_info.Joint1;
m_State.current_joint_2 = current_info.Joint2;
m_State.current_joint_3 = current_info.Joint3;
m_State.current_joint_4 = current_info.Joint4;
m_State.current_joint_5 = current_info.Joint5;
m_State.current_joint_6 = current_info.Joint6;
// getting fingers angles
m_State.finger_1.angle = (joint_info.Finger1) * Constants.DTR;
m_State.finger_2.angle = (joint_info.Finger2) * Constants.DTR;
m_State.finger_3.angle = (joint_info.Finger3) * Constants.DTR;
// getting finger current
m_State.current_finger_1 = current_info.Finger1;
m_State.current_finger_2 = current_info.Finger2;
m_State.current_finger_3 = current_info.Finger3;
// getting the pose
// !!! CAUTION !!!!
// Jaco arm API is calculating the forward kinematics not fast enough or its not
// calculating at all...
//
// todo: need to replace by jaco_arm_kinematics FK
m_State.hand_position_x = pose_info.X ;
m_State.hand_position_y = pose_info.Y ;
m_State.hand_position_z = pose_info.Z ;
m_State.hand_orientation_x = pose_info.ThetaX ;
m_State.hand_orientation_y = pose_info.ThetaY ;
m_State.hand_orientation_z = pose_info.ThetaZ ;
// getting the trajectory info
m_State.current_traj = trajectory_info.StillInFIFO;
//}
}
catch (Exception ex)
{
System.Console.WriteLine("EXCEPTION in JacoRefreshEncoder");
System.Console.WriteLine(ex.ToString());
}
}
/// <summary>
/// 格子位置转格子下标
/// </summary>
/// <param name="gridPos"></param>
/// <returns></returns>
public int GetGridIdxFromGridPos(CPosition <int> gridPos)
{
return(gridPos.x * _grid.GridWidth + gridPos.y);
}
/// \brief Update the internal state, send commands.
public void Update()
{
CVectorAngle jp = m_Arm.ConfigurationsManager.GetJointPositions();
CVectorEuler hand = m_Arm.ConfigurationsManager.GetHandPosition();
// See the DH specs for angle conversions.
m_State.shoulder_yaw.angle =
(180.0 - jp.Angle[0]) / (180.0 / Math.PI);
m_State.shoulder_pitch.angle =
(jp.Angle[1] - 270.0) / (180.0 / Math.PI);
m_State.elbow_pitch.angle =
(90.0 - jp.Angle[2]) / (180.0 / Math.PI);
m_State.elbow_roll.angle =
(180.0 - jp.Angle[3]) / (180.0 / Math.PI);
m_State.wrist_roll.angle =
(180.0 - jp.Angle[4]) / (180.0 / Math.PI);
m_State.hand_roll.angle =
(260.0 - jp.Angle[5]) / (180.0 / Math.PI);
//Hand pose & orientation
m_State.hand_position_x = hand.Position[0];
m_State.hand_position_y = hand.Position[1];
m_State.hand_position_z = hand.Position[2];
m_State.hand_orientation_x = hand.Rotation[0];
m_State.hand_orientation_y = hand.Rotation[1];
m_State.hand_orientation_z = hand.Rotation[2];
//Update finger information (slow)
float[] fingerPos = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().UserPosition.FingerPosition;
//Update finger angles
m_State.finger_1.angle = fingerPos[0] * Math.PI / 180.0;
m_State.finger_2.angle = fingerPos[1] * Math.PI / 180.0;
m_State.finger_3.angle = fingerPos[2] * Math.PI / 180.0;
//if (m_IsRetracting && ((DateTime.Now - m_LastCmd) > m_RetractDelay))
if (m_IsRetracting)
{
try
{
//Getting position Live Only when retracting and wait for
CPosition positionLive = new CPosition();
positionLive = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
//System.Console.WriteLine("Retract status ");
//System.Console.WriteLine(positionLive.SystemStatus.RetractStatus);
/*
* Mode_Normal_To_Ready
* Mode_Ready_Standby
* Mode_Ready_To_Retract
* Mode_Retract_Standby
* Mode_Retract_To_Ready
* Mode_Normal
* Mode_NoInit_To_Ready
* ERROR
*/
//Are we done retracting
if (
positionLive.SystemStatus.RetractStatus == 1 ||
positionLive.SystemStatus.RetractStatus == 3)
{
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
else
{
//Look for timeout
if ((DateTime.Now - m_LastCmd) > m_RetractDelay)
{
System.Console.WriteLine("Retract delay expired");
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
}
}
catch (Exception e)
{
System.Console.WriteLine(
"JACO API diagnostic API Failed : ");
System.Console.WriteLine(e.ToString());
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
return;
}
// 200 ms watchdog, reset the command if it's too old.
if ((DateTime.Now - m_LastCmd) > m_WatchDogDelay)
{
m_Cmd = new CJoystickValue();
}
// NOTE: Disabled for now, testing trajectory mode.
//m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
}
/// <summary>
/// 找到所有能看到自己的Entity集合
/// </summary>
/// <param name="self"></param>
/// <param name="deep"></param>
/// <param name="curPos"></param>
/// <param name="gridIdxs"></param>
/// <param name="entities"></param>
public void GetEntitiesFromSight(EEntity self, CPosition <double> curPos, out HashSet <int> gridIdxs, out HashSet <EEntity> entities)
{
GetGridPosFromMapPos(curPos, out var gridPos);
GetEntitiesFromSight(self.SightDistance, gridPos, out gridIdxs, out entities);
}
