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); }