private void SetTrajectoryInfoMap(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
{
for (int i = 0; i < msg.joint_names.Count; i++)
{
HSRCommon.Joint joint = (HSRCommon.Joint)Enum.Parse(typeof(HSRCommon.Joint), msg.joint_names[i]);
List <float> positions = new List <float>();
List <float> durations = new List <float>();
for (int pointIndex = 0; pointIndex < msg.points.Count; pointIndex++)
{
positions.Add(HSRCommon.GetClampedPosition((float)msg.points[pointIndex].positions[i], joint));
durations.Add((float)msg.points[pointIndex].time_from_start.secs + (float)msg.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
}
switch (joint)
{
case HSRCommon.Joint.arm_lift_joint: { this.SetJointTrajectoryPosition(joint, durations, positions, +this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ); break; }
case HSRCommon.Joint.arm_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.armFlexLink.localEulerAngles.y); break; }
case HSRCommon.Joint.arm_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -this.armRollLink.localEulerAngles.z); break; }
case HSRCommon.Joint.wrist_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.wristFlexLink.localEulerAngles.y); break; }
case HSRCommon.Joint.wrist_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -this.wristRollLink.localEulerAngles.z); break; }
case HSRCommon.Joint.head_pan_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -this.headPanLink.localEulerAngles.z); break; }
case HSRCommon.Joint.head_tilt_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.headTiltLink.localEulerAngles.y); break; }
case HSRCommon.Joint.hand_motor_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.handMotorDummyLink.localEulerAngles.x); break; }
}
}
}
private void SetTrajectoryInfoMap(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
{
for (int i = 0; i < msg.joint_names.Count; i++)
{
HSRCommon.Joint joint = (HSRCommon.Joint)Enum.Parse(typeof(HSRCommon.Joint), msg.joint_names[i]);
List <float> positions = new List <float>();
List <float> durations = new List <float>();
for (int pointIndex = 0; pointIndex < msg.points.Count; pointIndex++)
{
positions.Add(HSRCommon.GetClampedPosition((float)msg.points[pointIndex].positions[i], joint));
durations.Add((float)msg.points[pointIndex].time_from_start.secs + (float)msg.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
}
switch (joint)
{
case HSRCommon.Joint.odom_x:
case HSRCommon.Joint.odom_y:
case HSRCommon.Joint.odom_t: { this.trajectoryInfoMap[joint] = new TrajectoryData(Time.time, durations, positions); break; }
default: { throw new Exception("NOT odom joint"); }
}
}
}
private void UpdateLinkAngle(Transform link, HSRCommon.Joint joint, Vector3 axis)
{
float newPos = HSRCommon.GetNormalizedJointEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint) * Mathf.Rad2Deg, joint);
if (Mathf.Abs(axis.x) == 1)
{
link.localEulerAngles = new Vector3(0.0f, link.localEulerAngles.y, link.localEulerAngles.z) + newPos * axis;
}
else if (Mathf.Abs(axis.y) == 1)
{
link.localEulerAngles = new Vector3(link.localEulerAngles.x, 0.0f, link.localEulerAngles.z) + newPos * axis;
}
else if (Mathf.Abs(axis.z) == 1)
{
link.localEulerAngles = new Vector3(link.localEulerAngles.x, link.localEulerAngles.y, 0.0f) + newPos * axis;
}
}
private static bool IsOverLimitSpeed(HSRCommon.Joint joint, double speed)
{
return(speed > HSRCommon.GetMaxJointSpeed(joint));
}
private void SetJointTrajectoryRotation(HSRCommon.Joint joint, List <float> durations, List <float> goalPositions, float currentPosition)
{
this.trajectoryInfoMap[joint] = new TrajectoryInfo(durations, goalPositions, HSRCommon.GetNormalizedJointEulerAngle(currentPosition, joint) * Mathf.Deg2Rad);
}
private void SetJointTrajectoryPosition(HSRCommon.Joint joint, List <float> durations, List <float> goalPositions, float currentPosition)
{
this.trajectoryInfoMap[joint] = new TrajectoryInfo(durations, goalPositions, currentPosition);
}
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);
}
