private void UpdateGripperPosition(Transform link, TIAGoCommon.Joint joint, float iniPosX, bool isLeft)
{
float newPos = TrajectoryInfo.GetPositionAndUpdateTrajectory <TIAGoCommon.Joint>(this.trajectoryInfoMap, joint, GetMinJointSpeed(joint), GetMaxJointSpeed(joint));
if (isLeft)
{
newPos = +(newPos - iniPosX);
}
else
{
newPos = -(newPos - iniPosX);
}
bool isGripperClosing = isLeft? (newPos < link.localPosition.x) : (newPos > link.localPosition.x);
// Grasping and gripper closing
if (this.graspedObject != null && isGripperClosing)
{
// Have to stop
this.trajectoryInfoMap[joint] = null;
}
// Otherwise
else
{
link.localPosition = new Vector3(iniPosX + newPos, link.localPosition.y, link.localPosition.z);
}
}
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);
}
protected void FixedUpdate()
{
foreach (TIAGoCommon.Joint joint in this.trajectoryKeyList)
{
if (this.trajectoryInfoMap[joint] == null)
{
continue;
}
switch (joint)
{
case TIAGoCommon.Joint.torso_lift_joint:
{
float newPos = TrajectoryInfo.GetPositionAndUpdateTrajectory <TIAGoCommon.Joint>(this.trajectoryInfoMap, joint, GetMinJointSpeed(joint), GetMaxJointSpeed(joint));
this.torsoLiftLink.localPosition = new Vector3(this.torsoLiftLink.localPosition.x, this.torsoLiftLink.localPosition.y, this.torsoLiftLinkIniPosZ + newPos);
break;
}
case TIAGoCommon.Joint.head_1_joint: { this.UpdateLinkAngle(this.head1Link, joint, 0.0f, Vector3.back); break; }
case TIAGoCommon.Joint.head_2_joint: { this.UpdateLinkAngle(this.head2Link, joint, 0.0f, Vector3.up); break; }
case TIAGoCommon.Joint.arm_1_joint: { this.UpdateLinkAngle(this.arm1Link, joint, this.arm1LinkIniRotZ, Vector3.back); break; }
case TIAGoCommon.Joint.arm_2_joint: { this.UpdateLinkAngle(this.arm2Link, joint, 0.0f, Vector3.up); break; }
case TIAGoCommon.Joint.arm_3_joint: { this.UpdateLinkAngle(this.arm3Link, joint, this.arm3LinkIniRotZ, Vector3.back); break; }
case TIAGoCommon.Joint.arm_4_joint: { this.UpdateLinkAngle(this.arm4Link, joint, this.arm4LinkIniRotY, Vector3.down); break; }
case TIAGoCommon.Joint.arm_5_joint: { this.UpdateLinkAngle(this.arm5Link, joint, 0.0f, Vector3.back); break; }
case TIAGoCommon.Joint.arm_6_joint: { this.UpdateLinkAngle(this.arm6Link, joint, this.arm6LinkIniRotY, Vector3.down); break; }
case TIAGoCommon.Joint.arm_7_joint: { this.UpdateLinkAngle(this.arm7Link, joint, this.arm7LinkIniRotZ, Vector3.back); break; }
case TIAGoCommon.Joint.gripper_left_finger_joint: { this.UpdateGripperPosition(this.gripperLeftFingerLink, joint, this.gripperLeftFingerLinkIniPosX, true); break; }
case TIAGoCommon.Joint.gripper_right_finger_joint: { this.UpdateGripperPosition(this.gripperRightFingerLink, joint, this.gripperRightFingerLinkIniPosX, false); break; }
}
}
}