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"); }
}
}
}
public void JointTrajectoryCallback(SIGVerse.ROSBridge.trajectory_msgs.JointTrajectory jointTrajectory)
{
//List<string> jointNames = jointTrajectory.joint_names;
//List<SIGVerse.ROSBridge.trajectory_msgs.JointTrajectoryPoint> points = jointTrajectory.points;
if (jointTrajectory.joint_names.Count != jointTrajectory.points[0].positions.Count)
{
SIGVerseLogger.Warn("joint_names.Count != points.positions.Count topicName = " + this.topicName);
return;
}
const int Zero = 0;
for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
{
string name = jointTrajectory.joint_names[i];
float position = HSRCommon.GetClampedPosition((float)jointTrajectory.points[Zero].positions[i], name);
float duration = (float)jointTrajectory.points[Zero].time_from_start.secs + (float)jointTrajectory.points[Zero].time_from_start.nsecs * 1.0e-9f;
// Debug.Log("Duration="+ duration);
if (name == HSRCommon.ArmLiftJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
}
if (name == HSRCommon.ArmFlexJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.ArmRollJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.WristFlexJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.WristRollJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HeadPanJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HeadTiltJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HandLProximalJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HandRProximalJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
}
}
}
protected override void SubscribeMessageCallback(SIGVerse.RosBridge.trajectory_msgs.JointTrajectory jointTrajectory)
{
if (jointTrajectory.joint_names.Count != jointTrajectory.points[0].positions.Count)
{
SIGVerseLogger.Warn("joint_names.Count != points.positions.Count topicName = " + this.topicName);
return;
}
for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
{
string name = jointTrajectory.joint_names[i];
List <float> positions = new List <float>();
List <float> durations = new List <float>();
for (int pointIndex = 0; pointIndex < jointTrajectory.points.Count; pointIndex++)
{
positions.Add(HSRCommon.GetClampedPosition((float)jointTrajectory.points[pointIndex].positions[i], name));
durations.Add((float)jointTrajectory.points[pointIndex].time_from_start.secs + (float)jointTrajectory.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
}
if (name == HSRCommon.ArmLiftJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
}
if (name == HSRCommon.ArmFlexJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.ArmRollJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.WristFlexJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.WristRollJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HeadPanJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HeadTiltJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HandLProximalJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HandRProximalJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
}
}
}
protected override void SubscribeMessageCallback(SIGVerse.RosBridge.trajectory_msgs.JointTrajectory jointTrajectory)
{
this.startPosition = this.baseFootprintRigidbody.position;//save start position.
this.startRotation = this.baseFootprintRigidbody.rotation;
if (jointTrajectory.joint_names.Count != jointTrajectory.points[0].positions.Count)
{
SIGVerseLogger.Warn("joint_names.Count != points.positions.Count topicName = " + this.topicName);
return;
}
else if (jointTrajectory.joint_names.Count != 3)
{
SIGVerseLogger.Warn("joint_names.Count != 3");
return;
}
for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
{
string name = jointTrajectory.joint_names[i];
List <float> positions = new List <float>();
List <float> durations = new List <float>();
for (int pointIndex = 0; pointIndex < jointTrajectory.points.Count; pointIndex++)
{
positions.Add(HSRCommon.GetClampedPosition((float)jointTrajectory.points[pointIndex].positions[i], name));
durations.Add((float)jointTrajectory.points[pointIndex].time_from_start.secs + (float)jointTrajectory.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
}
if (name == HSRCommon.OmniOdomX_JointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.baseFootprintRigidbody.position.z);
}
if (name == HSRCommon.OmniOdomY_JointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.baseFootprintRigidbody.position.x);
}
if (name == HSRCommon.OmniOdomT_JointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, 0.0f);
}
}
if (trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] == null || trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] == null || trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] == null)
{
trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] = null;
SIGVerseLogger.Warn("Omni trajectry args error.");
}
//check limit speed
for (int i = 0; i < trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions.Count; i++)
{
double linear_speed = 0;
if (i == 0)
{
double temp_distance = Math.Sqrt(Math.Pow(((trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[0] + this.initialPosition.z) - this.startPosition.z) * 100, 2) + Math.Pow(((trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[0] + this.initialPosition.x) - this.startPosition.x) * 100, 2));
linear_speed = (temp_distance / 100) / trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[0];
}
else
{
double temp_distance = Math.Sqrt(Math.Pow((trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[i] - trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[i - 1]) * 100, 2) + Math.Pow((trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[i] - trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[i - 1]) * 100, 2));
linear_speed = (temp_distance / 100) / (trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[i] - trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[i - 1]);
}
double angular_speed = 0;
if (i == 0)
{
double temp_angle = Math.Abs((this.ToAngle(trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[0]) + this.initialRotation.eulerAngles.y) - this.startRotation.eulerAngles.y);
if (temp_angle > Math.PI)
{
temp_angle -= Math.PI;
}
angular_speed = temp_angle / trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[0];
}
else
{
double temp_angle = Math.Abs(this.ToAngle(trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[i]) - trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[i - 1]);
if (temp_angle > Math.PI)
{
temp_angle -= Math.PI;
}
angular_speed = temp_angle / (trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[i] - trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[i - 1]);
}
if (linear_speed > HSRCommon.MaxSpeedBase || angular_speed > this.ToAngle(HSRCommon.MaxSpeedBaseRad))//exceed limit
{
trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] = null;
SIGVerseLogger.Warn("Omni trajectry args error. (exceed limit)");
return;
}
} //for
} //SubscribeMessageCallback
