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>()
{
{ this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ }, //1 arm_lift_joint
{ HSRCommon.GetNormalizedJointEulerAngle(+this.armFlexLink.localEulerAngles.y, HSRCommon.Joint.arm_flex_joint) * Mathf.Deg2Rad }, // 2
{ HSRCommon.GetNormalizedJointEulerAngle(-this.armRollLink.localEulerAngles.z, HSRCommon.Joint.arm_roll_joint) * Mathf.Deg2Rad }, // 3
{ HSRCommon.GetNormalizedJointEulerAngle(+this.wristFlexLink.localEulerAngles.y, HSRCommon.Joint.wrist_flex_joint) * Mathf.Deg2Rad }, // 4
{ HSRCommon.GetNormalizedJointEulerAngle(-this.wristRollLink.localEulerAngles.z, HSRCommon.Joint.wrist_roll_joint) * Mathf.Deg2Rad }, // 5
{ HSRCommon.GetNormalizedJointEulerAngle(-this.headPanLink.localEulerAngles.z, HSRCommon.Joint.head_pan_joint) * Mathf.Deg2Rad }, // 6
{ HSRCommon.GetNormalizedJointEulerAngle(+this.headTiltLink.localEulerAngles.y, HSRCommon.Joint.head_tilt_joint) * Mathf.Deg2Rad }, // 7
{ HSRCommon.GetNormalizedJointEulerAngle(+this.handLProximalLink.localEulerAngles.x, HSRCommon.Joint.hand_l_proximal_joint) * Mathf.Deg2Rad }, // 8 hand_l_spring_proximal_joint
{ HSRCommon.GetNormalizedJointEulerAngle(+this.handRProximalLink.localEulerAngles.x, HSRCommon.Joint.hand_r_proximal_joint) * Mathf.Deg2Rad }, // 9 hand_r_spring_proximal_joint
};
this.jointState.header.Update();
this.jointState.position = positions;
this.publisher.Publish(this.jointState);
}
protected void FixedUpdate()
{
foreach (HSRCommon.Joint joint in this.trajectoryKeyList)
{
if (this.trajectoryInfoMap[joint] == null)
{
continue;
}
switch (joint)
{
case HSRCommon.Joint.arm_lift_joint:
{
float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint);
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);
break;
}
case HSRCommon.Joint.arm_flex_joint: { this.UpdateLinkAngle(this.armFlexLink, joint, Vector3.up); break; }
case HSRCommon.Joint.arm_roll_joint: { this.UpdateLinkAngle(this.armRollLink, joint, Vector3.back); break; }
case HSRCommon.Joint.wrist_flex_joint: { this.UpdateLinkAngle(this.wristFlexLink, joint, Vector3.up); break; }
case HSRCommon.Joint.wrist_roll_joint: { this.UpdateLinkAngle(this.wristRollLink, joint, Vector3.back); break; }
case HSRCommon.Joint.head_pan_joint: { this.UpdateLinkAngle(this.headPanLink, joint, Vector3.back); break; }
case HSRCommon.Joint.head_tilt_joint: { this.UpdateLinkAngle(this.headTiltLink, joint, Vector3.up); break; }
case HSRCommon.Joint.hand_motor_joint:
{
float newPos = HSRCommon.GetNormalizedJointEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint) * Mathf.Rad2Deg, joint);
// Grasping and hand closing
if (this.graspedObject != null && this.IsAngleDecreasing(newPos, this.handMotorDummyLink.localEulerAngles.x))
{
// Have to stop
this.trajectoryInfoMap[joint] = null;
}
// Otherwise
else
{
this.handMotorDummyLink.localEulerAngles = new Vector3(+newPos, this.handMotorDummyLink.localEulerAngles.y, this.handMotorDummyLink.localEulerAngles.z);
this.handLProximalLink.localEulerAngles = new Vector3(+newPos, this.handLProximalLink.localEulerAngles.y, this.handLProximalLink.localEulerAngles.z);
this.handRProximalLink.localEulerAngles = new Vector3(-newPos, this.handRProximalLink.localEulerAngles.y, this.handRProximalLink.localEulerAngles.z);
this.handLDistalLink.localEulerAngles = new Vector3(-newPos, this.handLDistalLink.localEulerAngles.y, this.handLDistalLink.localEulerAngles.z);
this.handRDistalLink.localEulerAngles = new Vector3(+newPos, this.handRDistalLink.localEulerAngles.y, this.handRDistalLink.localEulerAngles.z);
}
break;
}
}
}
}
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 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);
}
