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); }
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); }
private void SetJointTrajectoryPosition(TIAGoCommon.Joint joint, List <float> durations, List <float> goalPositions, float currentPosition) { this.trajectoryInfoMap[joint] = new TrajectoryInfo(durations, goalPositions, currentPosition); }