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>() { TIAGoCommon.GetNormalizedJointEulerAngle(-(this.arm1Link.localEulerAngles.z - this.arm1LinkIniRotZ), TIAGoCommon.Joint.arm_1_joint) * Mathf.Deg2Rad, // 1 TIAGoCommon.GetNormalizedJointEulerAngle(+(this.arm2Link.localEulerAngles.y), TIAGoCommon.Joint.arm_2_joint) * Mathf.Deg2Rad, // 2 TIAGoCommon.GetNormalizedJointEulerAngle(-(this.arm3Link.localEulerAngles.z - this.arm3LinkIniRotZ), TIAGoCommon.Joint.arm_3_joint) * Mathf.Deg2Rad, // 3 TIAGoCommon.GetNormalizedJointEulerAngle(-(this.arm4Link.localEulerAngles.y - this.arm4LinkIniRotY), TIAGoCommon.Joint.arm_4_joint) * Mathf.Deg2Rad, // 4 TIAGoCommon.GetNormalizedJointEulerAngle(-(this.arm5Link.localEulerAngles.z), TIAGoCommon.Joint.arm_5_joint) * Mathf.Deg2Rad, // 5 TIAGoCommon.GetNormalizedJointEulerAngle(-(this.arm6Link.localEulerAngles.y - this.arm6LinkIniRotY), TIAGoCommon.Joint.arm_6_joint) * Mathf.Deg2Rad, // 6 TIAGoCommon.GetNormalizedJointEulerAngle(-(this.arm7Link.localEulerAngles.z - this.arm7LinkIniRotZ), TIAGoCommon.Joint.arm_7_joint) * Mathf.Deg2Rad, // 7 +(this.gripperLeftFingerLink.localPosition.x - this.gripperLeftFingerLinkIniPosX), // 8 -(this.gripperRightFingerLink.localPosition.x - this.gripperRightFingerLinkIniPosX), // 9 TIAGoCommon.GetNormalizedJointEulerAngle(-this.head1Link.localEulerAngles.z, TIAGoCommon.Joint.head_1_joint) * Mathf.Deg2Rad, // 10 TIAGoCommon.GetNormalizedJointEulerAngle(+this.head2Link.localEulerAngles.y, TIAGoCommon.Joint.head_2_joint) * Mathf.Deg2Rad, // 11 +(this.torsoLiftLink.localPosition.z - this.torsoLiftLinkIniPosZ), // 12 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 // 13 -- 24 }; this.jointState.header.Update(); this.jointState.position = positions; this.publisher.Publish(this.jointState); }
private void UpdateLinkAngle(Transform link, TIAGoCommon.Joint joint, float iniRot, Vector3 axis) { float newPosRad = TrajectoryInfo.GetPositionAndUpdateTrajectory <TIAGoCommon.Joint>(this.trajectoryInfoMap, joint, TIAGoCommon.GetMinJointSpeed(joint), TIAGoCommon.GetMaxJointSpeed(joint)); float newPos = TIAGoCommon.GetNormalizedJointEulerAngle(newPosRad * Mathf.Rad2Deg, joint); TrajectoryInfo.UpdateLinkAngle(link, axis, newPos - iniRot); }
private void SetJointTrajectoryRotation(TIAGoCommon.Joint joint, List <float> durations, List <float> goalPositions, float currentPosition) { this.trajectoryInfoMap[joint] = new TrajectoryInfo(durations, goalPositions, TIAGoCommon.GetNormalizedJointEulerAngle(currentPosition, joint) * Mathf.Deg2Rad); }