protected override void Update()
{
base.Update();
this.elapsedTime += UnityEngine.Time.deltaTime;
if (this.elapsedTime < this.sendingInterval * 0.001)
{
return;
}
this.elapsedTime = 0.0f;
List <double> positions = new List <double>();
//1 ArmLiftJoint
positions.Add(this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
//2 ArmFlexJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, HSRCommon.ArmFlexJointName) * Mathf.Deg2Rad);
//3 ArmRollJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, HSRCommon.ArmRollJointName) * Mathf.Deg2Rad);
//4 WristFlexJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, HSRCommon.WristFlexJointName) * Mathf.Deg2Rad);
//5 WristRollJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, HSRCommon.WristRollJointName) * Mathf.Deg2Rad);
//6 HeadPanJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, HSRCommon.HeadPanJointName) * Mathf.Deg2Rad);
//7 HeadTiltJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, HSRCommon.HeadTiltJointName) * Mathf.Deg2Rad);
//8 HandLSpringProximalJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, HSRCommon.HandLProximalJointName) * Mathf.Deg2Rad);
//9 HandRSpringProximalJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, HSRCommon.HandRProximalJointName) * Mathf.Deg2Rad);
this.jointState.header.Update();
this.jointState.position = positions;
// float position = HSRCommon.GetClampedPosition(value, name);
this.publisher.Publish(this.jointState);
}
void Update()
{
if (this.webSocketConnection == null || !this.webSocketConnection.IsConnected)
{
return;
}
foreach (string jointName in this.trajectoryKeyList)
{
if (this.trajectoryInfoMap[jointName] != null)
{
if (jointName == HSRCommon.ArmLiftJointName)
{
float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, 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(this.trajectoryInfoMap, jointName, 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(this.trajectoryInfoMap, jointName, 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(this.trajectoryInfoMap, jointName, 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(this.trajectoryInfoMap, jointName, 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(this.trajectoryInfoMap, jointName, 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(this.trajectoryInfoMap, jointName, 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(this.trajectoryInfoMap, jointName, 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
this.trajectoryInfoMap[jointName] = 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(this.trajectoryInfoMap, jointName, 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
this.trajectoryInfoMap[jointName] = null;
}
// Otherwise
else
{
this.handRProximalLink.localEulerAngles = new Vector3(newPos, this.handRProximalLink.localEulerAngles.y, this.handRProximalLink.localEulerAngles.z);
}
}
}
}
this.webSocketConnection.Render();
}
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);
}
}
}
