private static int GetTargetPointIndex(TrajectoryInfo trajectoryInfo)
{
int targetPointIndex = 0;
for (int i = 0; i < trajectoryInfo.Durations.Count; i++)
{
targetPointIndex = i;
if (Time.time - trajectoryInfo.StartTime < trajectoryInfo.Durations[targetPointIndex])
{
break;
}
}
return(targetPointIndex);
}
private static float GetPositionAndUpdateTrajectory(Dictionary <HSRCommon.Joint, TrajectoryInfo> trajectoryInfoMap, HSRCommon.Joint joint)
{
float minSpeed = HSRCommon.GetMinJointSpeed(joint);
float maxSpeed = HSRCommon.GetMaxJointSpeed(joint);
TrajectoryInfo trajectoryInfo = trajectoryInfoMap[joint];
int targetPointIndex = GetTargetPointIndex(trajectoryInfo);
float speed = 0.0f;
if (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime >= trajectoryInfo.Durations[targetPointIndex])
{
speed = maxSpeed;
}
else
{
speed = Mathf.Abs((trajectoryInfo.GoalPositions[targetPointIndex] - trajectoryInfo.CurrentPosition) / (trajectoryInfo.Durations[targetPointIndex] - (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime)));
speed = Mathf.Clamp(speed, minSpeed, maxSpeed);
}
// Calculate position
float newPosition;
float movingDistance = speed * (Time.time - trajectoryInfo.CurrentTime);
if (movingDistance > Mathf.Abs(trajectoryInfo.GoalPositions[targetPointIndex] - trajectoryInfo.CurrentPosition))
{
newPosition = trajectoryInfo.GoalPositions[targetPointIndex];
trajectoryInfoMap[joint] = null;
}
else
{
trajectoryInfo.CurrentTime = Time.time;
if (trajectoryInfo.GoalPositions[targetPointIndex] > trajectoryInfo.CurrentPosition)
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition + movingDistance;
newPosition = trajectoryInfo.CurrentPosition;
}
else
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition - movingDistance;
newPosition = trajectoryInfo.CurrentPosition;
}
}
return(newPosition);
}
public static float GetPositionAndUpdateTrajectory(Dictionary <string, TrajectoryInfo> trajectoryInfoMap, string jointName, float minSpeed, float maxSpeed)
{
TrajectoryInfo trajectoryInfo = trajectoryInfoMap[jointName];
// Calculate move speed
float speed = 0.0f;
if (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime >= trajectoryInfo.Duration)
{
speed = maxSpeed;
}
else
{
speed = Mathf.Abs((trajectoryInfo.GoalPosition - trajectoryInfo.CurrentPosition) / (trajectoryInfo.Duration - (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime)));
speed = Mathf.Clamp(speed, minSpeed, maxSpeed);
}
// Calculate position
float newPosition;
float movingDistance = speed * (Time.time - trajectoryInfo.CurrentTime);
if (movingDistance > Mathf.Abs(trajectoryInfo.GoalPosition - trajectoryInfo.CurrentPosition))
{
newPosition = trajectoryInfo.GoalPosition;
trajectoryInfoMap[jointName] = null;
}
else
{
trajectoryInfo.CurrentTime = Time.time;
if (trajectoryInfo.GoalPosition > trajectoryInfo.CurrentPosition)
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition + movingDistance;
newPosition = trajectoryInfo.CurrentPosition;
}
else
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition - movingDistance;
newPosition = trajectoryInfo.CurrentPosition;
}
}
return(newPosition);
}
protected void FixedUpdate()
{
foreach (TIAGoCommon.Joint joint in this.trajectoryKeyList)
{
if (this.trajectoryInfoMap[joint] == null)
{
continue;
}
switch (joint)
{
case TIAGoCommon.Joint.torso_lift_joint:
{
float newPos = TrajectoryInfo.GetPositionAndUpdateTrajectory <TIAGoCommon.Joint>(this.trajectoryInfoMap, joint, GetMinJointSpeed(joint), GetMaxJointSpeed(joint));
this.torsoLiftLink.localPosition = new Vector3(this.torsoLiftLink.localPosition.x, this.torsoLiftLink.localPosition.y, this.torsoLiftLinkIniPosZ + newPos);
break;
}
case TIAGoCommon.Joint.head_1_joint: { this.UpdateLinkAngle(this.head1Link, joint, 0.0f, Vector3.back); break; }
case TIAGoCommon.Joint.head_2_joint: { this.UpdateLinkAngle(this.head2Link, joint, 0.0f, Vector3.up); break; }
case TIAGoCommon.Joint.arm_1_joint: { this.UpdateLinkAngle(this.arm1Link, joint, this.arm1LinkIniRotZ, Vector3.back); break; }
case TIAGoCommon.Joint.arm_2_joint: { this.UpdateLinkAngle(this.arm2Link, joint, 0.0f, Vector3.up); break; }
case TIAGoCommon.Joint.arm_3_joint: { this.UpdateLinkAngle(this.arm3Link, joint, this.arm3LinkIniRotZ, Vector3.back); break; }
case TIAGoCommon.Joint.arm_4_joint: { this.UpdateLinkAngle(this.arm4Link, joint, this.arm4LinkIniRotY, Vector3.down); break; }
case TIAGoCommon.Joint.arm_5_joint: { this.UpdateLinkAngle(this.arm5Link, joint, 0.0f, Vector3.back); break; }
case TIAGoCommon.Joint.arm_6_joint: { this.UpdateLinkAngle(this.arm6Link, joint, this.arm6LinkIniRotY, Vector3.down); break; }
case TIAGoCommon.Joint.arm_7_joint: { this.UpdateLinkAngle(this.arm7Link, joint, this.arm7LinkIniRotZ, Vector3.back); break; }
case TIAGoCommon.Joint.gripper_left_finger_joint: { this.UpdateGripperPosition(this.gripperLeftFingerLink, joint, this.gripperLeftFingerLinkIniPosX, true); break; }
case TIAGoCommon.Joint.gripper_right_finger_joint: { this.UpdateGripperPosition(this.gripperRightFingerLink, joint, this.gripperRightFingerLinkIniPosX, false); break; }
}
}
}
void test3()
{
ContactSolver cs = new ContactSolver();
Vector2 O = transform.TransformPoint(coll [0].points [0] + coll[0].offset);
Vector2 E = transform.TransformPoint(coll [0].points [1] + coll[0].offset);
Vector2 A = transform.TransformPoint(coll [1].points [0] + coll[1].offset);
Vector2 B = transform.TransformPoint(coll [1].points [1] + coll[1].offset);
float R = 0.2f;
Gizmos.DrawLine(O, E);
Gizmos.DrawLine(A, B);
TrajectoryInfo contact = cs.getFirstContact(O, E, A, B, R);
if (contact)
{
Gizmos.DrawSphere(contact.getPosition(), R);
Gizmos.DrawSphere(contact.A + contact.contactRatio * (contact.B - contact.A), 0.1f);
}
}
private void UpdateTrajInfo(out TrajectoryInfo traj)
{
traj = default(TrajectoryInfo);
Ray aimRay = base.GetAimRay();
RaycastHit rHit = default(RaycastHit);
Vector3 direction = aimRay.direction;
direction.y = Mathf.Max(direction.y, EntityStates.Croco.BaseLeap.minimumY);
Vector3 a = direction.normalized * EntityStates.Croco.BaseLeap.aimVelocity * this.moveSpeedStat;
Vector3 b = Vector3.up * EntityStates.Croco.BaseLeap.upwardVelocity;
Vector3 b2 = new Vector3(direction.x, 0f, direction.z).normalized *EntityStates.Croco.BaseLeap.forwardVelocity;
Vector3 velocity = a + b + b2;
Vector3 tempVelocity = velocity;
Ray r = new Ray(aimRay.origin, velocity);
Single time = Trajectory.CalculateFlightDuration(velocity.y);
Single timeStep = time / 25f;
Vector3 oldPos = aimRay.origin;
Boolean hit = false;
List <Vector3> vecs = new List <Vector3>(100);
for (Int32 i = 0; i < 100; i++)
{
Vector3 pos = Trajectory.CalculatePositionAtTime(r.origin, tempVelocity, timeStep * i, Physics.gravity.y);
tempVelocity *= Mathf.Exp(timeStep * -0.4f);
hit = Physics.SphereCast(oldPos, 0.2f, (pos - oldPos), out rHit, (pos - oldPos).magnitude, LayerIndex.world.mask | LayerIndex.entityPrecise.mask, QueryTriggerInteraction.UseGlobal);
if (hit)
{
break;
}
oldPos = pos;
vecs.Add(pos);
}
arcLineRender.positionCount = vecs.Count;
arcLineRender.SetPositions(vecs.ToArray());
if (hit)
{
traj.hitPoint = rHit.point;
traj.hitNormal = rHit.normal;
}
else
{
traj.hitPoint = oldPos;
traj.hitNormal = Vector3.up;
}
Vector3 diff = traj.hitPoint - aimRay.origin;
if (useGravity)
{
Single speed = velocity.magnitude;
Vector2 flatDiff = new Vector2(diff.x, diff.z);
Single flatDist = flatDiff.magnitude;
Single yStart = Trajectory.CalculateInitialYSpeed(flatDist / speed, diff.y);
Vector3 speedVec = new Vector3(flatDiff.x / flatDist * speed, yStart, flatDiff.y / flatDist * speed);
traj.speedOverride = speedVec.magnitude;
traj.finalRay = new Ray(aimRay.origin, speedVec / traj.speedOverride);
traj.travelTime = Trajectory.CalculateGroundTravelTime(speed, flatDist);
return;
}
traj.speedOverride = baseSpeed;
traj.finalRay = aimRay;
traj.travelTime = baseSpeed / diff.magnitude;
}
public void PreProcess()
{
Debug.Log("Creating Asset");
string path = "Assets/Resources/MotionData";
if (!AssetDatabase.IsValidFolder(path))
{
if (!AssetDatabase.IsValidFolder("Assets/Resources"))
{
AssetDatabase.CreateFolder("Assets", "Resources");
}
AssetDatabase.CreateFolder("Assets/Resources", "MotionData");
}
//AnimationScriptableObjectContainer w = new AnimationScriptableObjectContainer();
MMMotionData motionAsset = CreateInstance <MMMotionData>();
motionAsset = CreateOrReplaceAsset(motionAsset, path + "/MotionData.asset");
int clipsLength = 0;
List <AnimLookup> lookupList = new List <AnimLookup>();
List <MotionFrameInfo> frameList = new List <MotionFrameInfo>();
foreach (var mmAnimationClip in mmAnimationClips)
{
// Create satellite data for animation lookup
lookupList.Add(new AnimLookup(clipsLength, mmAnimationClip.animClip.name));
clipsLength += mmAnimationClip.Length;
//Create combined array I guess..
for (int i = 0; i < mmAnimationClip.Length; i++)
{
Vector3[] positionPoints = new Vector3[config.trajectoryTimePoints.Count];
Vector3[] forwardPoints = new Vector3[config.trajectoryTimePoints.Count];
bool isInvalid = false;
for (int j = 0; j < config.trajectoryTimePoints.Count; j++)
{
int projectedIndex = Mathf.RoundToInt(config.trajectoryTimePoints[j] * config.frameRate) + i;
if (projectedIndex < mmAnimationClip.Length && projectedIndex >= 0)
{
positionPoints[j] = mmAnimationClip
.baseTrajectory[i].rootWorldToLocalMatrix.MultiplyPoint3x4(mmAnimationClip
.baseTrajectory[
Mathf.RoundToInt(config.trajectoryTimePoints[j] * config.frameRate) + i]
.position);
forwardPoints[j] = mmAnimationClip
.baseTrajectory[i].rootWorldToLocalMatrix.MultiplyVector(mmAnimationClip
.baseTrajectory[
Mathf.RoundToInt(config.trajectoryTimePoints[j] * config.frameRate) + i]
.forward);
}
else
{
isInvalid = true;
}
}
if (isInvalid)
{
clipsLength--;
continue;
}
List <Vector3> charSpacePositions = new List <Vector3>();
List <Vector3> charSpaceVelocities = new List <Vector3>();
for (int j = 0; j < mmAnimationClip.basePoses[i].jointPositions.Count; j++)
{
Vector3 tempPos = mmAnimationClip.baseTrajectory[i].rootWorldToLocalMatrix
.MultiplyVector(mmAnimationClip.basePoses[i].jointPositions[j]);
Vector3 tempVel = mmAnimationClip.baseTrajectory[i].rootWorldToLocalMatrix
.MultiplyVector(mmAnimationClip.basePoses[i].jointVelocities[j]);
charSpacePositions.Add(tempPos);
charSpaceVelocities.Add(tempVel);
}
TrajectoryInfo tempTraj = new TrajectoryInfo(positionPoints, forwardPoints);
Pose tempPose = new Pose(charSpacePositions, charSpaceVelocities);
frameList.Add(new MotionFrameInfo(tempTraj, tempPose, 0));
}
}
/*motionAsset.trajectoryInfo = trajectories;
* motionAsset.poseInfo = poses;*/
motionAsset.config = config;
motionAsset.Length = clipsLength;
motionAsset.frameInfo = frameList.ToArray();
motionAsset.satelliteData = lookupList.ToArray();
AssetDatabase.SaveAssets();
CreateAnimatorController();
}
private static float GetPositionAndUpdateTrajectory(Dictionary <PR2Common.Joint, TrajectoryInfo> trajectoryInfoMap, PR2Common.Joint joint)
{
float minSpeed = PR2Common.GetMinJointSpeed(joint);
float maxSpeed = PR2Common.GetMaxJointSpeed(joint);
TrajectoryInfo trajectoryInfo = trajectoryInfoMap[joint];
int targetPointIndex = GetTargetPointIndex(trajectoryInfo);
float goalPosition = trajectoryInfo.GoalPositions[targetPointIndex];
if (PR2Common.continuousJoints.Contains(joint))
{
if (goalPosition - trajectoryInfo.CurrentPosition > +Mathf.PI)
{
goalPosition -= 2 * Mathf.PI;
}
if (goalPosition - trajectoryInfo.CurrentPosition <= -Mathf.PI)
{
goalPosition += 2 * Mathf.PI;
}
}
float speed = 0.0f;
if (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime >= trajectoryInfo.Durations[targetPointIndex])
{
speed = maxSpeed;
}
else
{
speed = Mathf.Abs((goalPosition - trajectoryInfo.CurrentPosition) / (trajectoryInfo.Durations[targetPointIndex] - (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime)));
speed = Mathf.Clamp(speed, minSpeed, maxSpeed);
}
// Calculate position
float newPosition;
float movingDistance = speed * (Time.time - trajectoryInfo.CurrentTime);
if (movingDistance > Mathf.Abs(goalPosition - trajectoryInfo.CurrentPosition))
{
newPosition = goalPosition;
trajectoryInfoMap[joint] = null;
}
else
{
trajectoryInfo.CurrentTime = Time.time;
if (goalPosition > trajectoryInfo.CurrentPosition)
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition + movingDistance;
}
else
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition - movingDistance;
}
if (PR2Common.continuousJoints.Contains(joint))
{
trajectoryInfo.CurrentPosition = GetNormalizedRadian(trajectoryInfo.CurrentPosition);
}
newPosition = trajectoryInfo.CurrentPosition;
}
return(newPosition);
}
private ContactInfo findContact(Vector2 movement)
{
Vector2 start = transform.position;
Vector2 end = (Vector2)transform.position + movement;
Collider2D[] colliders = Physics2D.OverlapAreaAll(new Vector2(Mathf.Min(start.x, end.x) - 1, Mathf.Min(start.y, end.y) - 1),
new Vector2(Mathf.Max(start.x, end.x) + 1, Mathf.Max(start.y, end.y) + 1));
float minDistance = Mathf.Infinity;
TrajectoryInfo minTi = null;
EdgeCollider2D minEdge = null;
int minPoint = -1;
for (int i = 0; i < colliders.Length; i++)
{
if (colliders[i].GetType() == typeof(EdgeCollider2D))
{
EdgeCollider2D edge = (EdgeCollider2D)colliders[i];
for (int j = 0; j < edge.points.Length - 1; j++)
{
bool canCollide = (contactCount < 1 || contactInfos[0].acceptableEdge(edge, j)) &&
(contactCount < 2 || contactInfos[1].acceptableEdge(edge, j));
//&& tester != edge;
if (canCollide)
{
TrajectoryInfo ti = contactSolver.getFirstContact(start,
end,
edge.transform.TransformPoint(edge.points[j] + edge.offset),
edge.transform.TransformPoint(edge.points[j + 1] + edge.offset),
radius);
if (ti)
{
Vector2 tangent = edge.points[j + 1] - edge.points[j];
Vector2 normal = new Vector2(-tangent.y, tangent.x);
if (edge.gameObject.tag != "OneWay" || (ti.side && Vector2.Dot(movement, normal) < 0))
{
float distance = Vector2.Distance(start, ti.getPosition());
if (distance < minDistance)
{
minTi = ti;
minDistance = distance;
minEdge = edge;
minPoint = j;
}
}
}
}
}
}
}
if (minTi)
{
return(new ContactInfo(this,
minTi.contactRatio,
minEdge,
minPoint,
minTi.side,
minTi.isEdgeContact,
minTi.angle));
}
else
{
return(null);
}
}
void Update()
{
if (this.webSocketConnection == null || !this.webSocketConnection.IsConnected)
{
return;
}
foreach (KeyValuePair <string, TrajectoryInfo> trajectoryPair in trajectoryInfoMap)
{
string jointName = trajectoryPair.Key;
if (trajectoryPair.Value != null)
{
TrajectoryInfo trajectoryInfo = this.trajectoryInfoMap[jointName];
if (jointName == HSRCommon.ArmLiftJointName)
{
float newPos = GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeed, HSRCommon.MaxSpeedTorso);
this.armLiftLink.localPosition = new Vector3(this.armLiftLink.localPosition.x, this.armLiftLink.localPosition.y, this.armLiftLinkIniPosZ + newPos);
this.torsoLiftLink.localPosition = new Vector3(this.torsoLiftLink.localPosition.x, this.torsoLiftLink.localPosition.y, this.torsoLiftLinkIniPosZ + newPos / 2.0f);
}
if (jointName == HSRCommon.ArmFlexJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
this.armFlexLink.localEulerAngles = new Vector3(this.armFlexLink.localEulerAngles.x, newPos, this.armFlexLink.localEulerAngles.z);
}
if (jointName == HSRCommon.ArmRollJointName)
{
float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
this.armRollLink.localEulerAngles = new Vector3(this.armRollLink.localEulerAngles.x, this.armRollLink.localEulerAngles.y, newPos);
}
if (jointName == HSRCommon.WristFlexJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
this.wristFlexLink.localEulerAngles = new Vector3(this.wristFlexLink.localEulerAngles.x, newPos, this.wristFlexLink.localEulerAngles.z);
}
if (jointName == HSRCommon.WristRollJointName)
{
float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
this.wristRollLink.localEulerAngles = new Vector3(this.wristRollLink.localEulerAngles.x, this.wristRollLink.localEulerAngles.y, newPos);
}
if (jointName == HSRCommon.HeadPanJointName)
{
float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedHead) * Mathf.Rad2Deg, jointName);
this.headPanLink.localEulerAngles = new Vector3(this.headPanLink.localEulerAngles.x, this.headPanLink.localEulerAngles.y, newPos);
}
if (jointName == HSRCommon.HeadTiltJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedHead) * Mathf.Rad2Deg, jointName);
this.headTiltLink.localEulerAngles = new Vector3(this.headTiltLink.localEulerAngles.x, newPos, this.headTiltLink.localEulerAngles.z);
}
if (jointName == HSRCommon.HandLProximalJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
// Grasping and hand closing
if (this.graspedObject != null && this.IsAngleDecreasing(newPos, this.handLProximalLink.localEulerAngles.x))
{
// Have to stop
trajectoryInfo = null;
}
// Otherwise
else
{
this.handLProximalLink.localEulerAngles = new Vector3(newPos, this.handLProximalLink.localEulerAngles.y, this.handLProximalLink.localEulerAngles.z);
}
}
if (jointName == HSRCommon.HandRProximalJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(ref trajectoryInfo, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
// Grasping and hand closing
if (this.graspedObject != null && this.IsAngleIncreasing(newPos, this.handRProximalLink.localEulerAngles.x))
{
// Have to stop
trajectoryInfo = null;
}
// Otherwise
else
{
this.handRProximalLink.localEulerAngles = new Vector3(newPos, this.handRProximalLink.localEulerAngles.y, this.handRProximalLink.localEulerAngles.z);
}
}
}
}
this.webSocketConnection.Render();
}
}//Update
private void GetOmniXY_NewPosition(ref Dictionary <string, TrajectoryInfo> trajectoryInfoMap, ref Vector3 Position, ref Vector3 NoisePos)
{
TrajectoryInfo trajectoryInfo_x = trajectoryInfoMap[HSRCommon.OmniOdomX_JointName];
TrajectoryInfo trajectoryInfo_y = trajectoryInfoMap[HSRCommon.OmniOdomY_JointName];
int targetPointIndex = 0;
for (int i = 0; i < trajectoryInfo_x.Durations.Count; i++)//trajectryの対象ポイントを探査
{
targetPointIndex = i;
if (Time.time - trajectoryInfo_x.StartTime < trajectoryInfo_x.Durations[targetPointIndex])
{
break;
}
}
float delta_time;//時間進捗を0-1の時間にマッピング
if (targetPointIndex == 0)
{
delta_time = (Time.time - trajectoryInfo_x.StartTime) / (trajectoryInfo_x.Durations[targetPointIndex]);
}
else
{
delta_time = (Time.time - (trajectoryInfo_x.StartTime + trajectoryInfo_x.Durations[targetPointIndex - 1])) / (trajectoryInfo_x.Durations[targetPointIndex] - trajectoryInfo_x.Durations[targetPointIndex - 1]);
this.startPosition = new Vector3(trajectoryInfo_y.GoalPositions[targetPointIndex - 1] + this.initialPosition.x, 0, trajectoryInfo_x.GoalPositions[targetPointIndex - 1] + this.initialPosition.z);
}
Vector3 goalPosition = new Vector3();//temp xy goal point
goalPosition.x = trajectoryInfo_y.GoalPositions[targetPointIndex] + this.initialPosition.x;
goalPosition.z = trajectoryInfo_x.GoalPositions[targetPointIndex] + this.initialPosition.z;
if (delta_time > 1)
{
Position = (goalPosition - this.baseFootprintRigidbody.position);
if ((targetPointIndex + 1) == trajectoryInfo_x.GoalPositions.Count)
{
trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] = null;
}
return;
}
Position = Vector3.Slerp(this.startPosition, goalPosition, delta_time);//calc new position.
Position = (Position - this.baseFootprintRigidbody.position);
NoisePos.x = this.GetPosNoise(Position.x);
NoisePos.z = this.GetPosNoise(Position.z);
trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].CurrentPosition = Position.z;
trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].CurrentPosition = Position.x;
trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].CurrentTime = Time.time;
trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].CurrentTime = Time.time;
Debug.Log(Position.x);
return;
}//GetOmniXY_NewPosition
public MotionFrameInfo(TrajectoryInfo trajectoryInfo, Pose pose, int tag)
{
this.trajectoryInfo = trajectoryInfo;
this.pose = pose;
this.tag = tag;
}
