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);
}
protected override void InitializeVariables()
{
this.odomName = TIAGoCommon.OdomName;
this.baseFootprintName = TIAGoCommon.Link.base_footprint.ToString();
this.baseFootprintRigidbodyName = TIAGoCommon.BaseFootPrintRigidbodyName;
this.linkList = TIAGoCommon.GetLinksInChildren(this.transform.root);
}
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 SetTrajectoryInfoMap(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
{
for (int i = 0; i < msg.joint_names.Count; i++)
{
TIAGoCommon.Joint joint = (TIAGoCommon.Joint)Enum.Parse(typeof(TIAGoCommon.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(TIAGoCommon.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 TIAGoCommon.Joint.torso_lift_joint: { this.SetJointTrajectoryPosition(joint, durations, positions, +(this.torsoLiftLink.localPosition.z - this.torsoLiftLinkIniPosZ)); break; }
case TIAGoCommon.Joint.head_1_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -this.head1Link.localEulerAngles.z); break; }
case TIAGoCommon.Joint.head_2_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.head2Link.localEulerAngles.y); break; }
case TIAGoCommon.Joint.arm_1_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -(this.arm1Link.localEulerAngles.z - arm1LinkIniRotZ)); break; }
case TIAGoCommon.Joint.arm_2_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +(this.arm2Link.localEulerAngles.y)); break; }
case TIAGoCommon.Joint.arm_3_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -(this.arm3Link.localEulerAngles.z - arm3LinkIniRotZ)); break; }
case TIAGoCommon.Joint.arm_4_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -(this.arm4Link.localEulerAngles.y - arm4LinkIniRotY)); break; }
case TIAGoCommon.Joint.arm_5_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -(this.arm5Link.localEulerAngles.z)); break; }
case TIAGoCommon.Joint.arm_6_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -(this.arm6Link.localEulerAngles.y - arm6LinkIniRotY)); break; }
case TIAGoCommon.Joint.arm_7_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -(this.arm7Link.localEulerAngles.z - arm7LinkIniRotZ)); break; }
case TIAGoCommon.Joint.gripper_left_finger_joint: { this.SetJointTrajectoryPosition(joint, durations, positions, +(this.gripperLeftFingerLink.localPosition.x - gripperLeftFingerLinkIniPosX)); break; }
case TIAGoCommon.Joint.gripper_right_finger_joint: { this.SetJointTrajectoryPosition(joint, durations, positions, -(this.gripperRightFingerLink.localPosition.x - gripperRightFingerLinkIniPosX)); break; }
}
}
}
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);
}
