public ContinuousJointInfo(PR2Common.Joint joint, int listNum) { this.Joint = joint; this.SeqNo = listNum; this.PreviousPos = 0.0; this.RotationCount = 0; }
private float GetGripperNewPosition() { PR2Common.Joint joint = (this.handType == HandType.Left)? PR2Common.Joint.l_gripper_joint : PR2Common.Joint.r_gripper_joint; float maxDistance = 0.05f * Time.fixedDeltaTime; // speed=0.05[m/s] float newPos = Mathf.Clamp((float)this.gripperCommand.position, this.gripperCurrentPos - maxDistance, this.gripperCurrentPos + maxDistance); return(PR2Common.GetClampedPosition(newPos, joint)); }
private void SetTrajectoryInfoMap(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg) { for (int i = 0; i < msg.joint_names.Count; i++) { PR2Common.Joint joint = (PR2Common.Joint)Enum.Parse(typeof(PR2Common.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(PR2Common.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 PR2Common.Joint.torso_lift_joint: { this.SetJointTrajectoryPosition(joint, durations, positions, this.torsoLiftLink.localPosition.z - this.torsoLiftLinkIniPosZ); break; } case PR2Common.Joint.head_pan_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.back, this.headPanLink)); break; } case PR2Common.Joint.head_tilt_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.down, this.headTiltLink)); break; } case PR2Common.Joint.l_shoulder_pan_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.back, this.lShoulderPanLink)); break; } case PR2Common.Joint.l_shoulder_lift_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.down, this.lShoulderLiftLink)); break; } case PR2Common.Joint.l_upper_arm_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.right, this.lUpperArmRollLink)); break; } case PR2Common.Joint.l_elbow_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.down, this.lElbowFlexLink)); break; } case PR2Common.Joint.l_forearm_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.right, this.lForearmRollLink)); break; } case PR2Common.Joint.l_wrist_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.down, this.lWristFlexLink)); break; } case PR2Common.Joint.l_wrist_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.right, this.lWristRollLink)); break; } case PR2Common.Joint.r_shoulder_pan_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.back, this.rShoulderPanLink)); break; } case PR2Common.Joint.r_shoulder_lift_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.down, this.rShoulderLiftLink)); break; } case PR2Common.Joint.r_upper_arm_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.right, this.rUpperArmRollLink)); break; } case PR2Common.Joint.r_elbow_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.down, this.rElbowFlexLink)); break; } case PR2Common.Joint.r_forearm_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.right, this.rForearmRollLink)); break; } case PR2Common.Joint.r_wrist_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.down, this.rWristFlexLink)); break; } case PR2Common.Joint.r_wrist_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, PR2Common.GetRosAngleRad(joint, Vector3.right, this.rWristRollLink)); break; } } } }
private void SetJointTrajectoryRotation(PR2Common.Joint joint, List <float> durations, List <float> goalPositions, float currentPosition) { if (PR2Common.continuousJoints.Contains(joint)) { for (int i = 0; i < goalPositions.Count; i++) { goalPositions[i] = GetNormalizedRadian(goalPositions[i]); } currentPosition = GetNormalizedRadian(currentPosition); } this.trajectoryInfoMap[joint] = new TrajectoryInfo(durations, goalPositions, currentPosition); }
private void UpdateLinkPosition(Transform link, PR2Common.Joint joint, Vector3 axis, float initialPosition) { float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint); if (Mathf.Abs(axis.x) == 1) { link.localPosition = new Vector3(initialPosition, link.localPosition.y, link.localPosition.z) + newPos * axis; } else if (Mathf.Abs(axis.y) == 1) { link.localPosition = new Vector3(link.localPosition.x, initialPosition, link.localPosition.z) + newPos * axis; } else if (Mathf.Abs(axis.z) == 1) { link.localPosition = new Vector3(link.localPosition.x, link.localPosition.y, initialPosition) + newPos * axis; } }
private static bool IsOverLimitSpeed(PR2Common.Joint joint, double speed) { return(speed > PR2Common.GetMaxJointSpeed(joint)); }
private void SetJointTrajectoryPosition(PR2Common.Joint joint, List <float> durations, List <float> goalPositions, float currentPosition) { this.trajectoryInfoMap[joint] = new TrajectoryInfo(durations, goalPositions, currentPosition); }
private static float GetPositionAndUpdateTrajectory(Dictionary <PR2Common.Joint, TrajectoryInfo> trajectoryInfoMap, PR2Common.Joint joint) { float minSpeed = PR2Common.GetMinJointSpeed(joint); float maxSpeed = PR2Common.GetMaxJointSpeed(joint); TrajectoryInfo trajectoryInfo = trajectoryInfoMap[joint]; int targetPointIndex = GetTargetPointIndex(trajectoryInfo); float goalPosition = trajectoryInfo.GoalPositions[targetPointIndex]; if (PR2Common.continuousJoints.Contains(joint)) { if (goalPosition - trajectoryInfo.CurrentPosition > +Mathf.PI) { goalPosition -= 2 * Mathf.PI; } if (goalPosition - trajectoryInfo.CurrentPosition <= -Mathf.PI) { goalPosition += 2 * Mathf.PI; } } float speed = 0.0f; if (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime >= trajectoryInfo.Durations[targetPointIndex]) { speed = maxSpeed; } else { speed = Mathf.Abs((goalPosition - trajectoryInfo.CurrentPosition) / (trajectoryInfo.Durations[targetPointIndex] - (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime))); speed = Mathf.Clamp(speed, minSpeed, maxSpeed); } // Calculate position float newPosition; float movingDistance = speed * (Time.time - trajectoryInfo.CurrentTime); if (movingDistance > Mathf.Abs(goalPosition - trajectoryInfo.CurrentPosition)) { newPosition = goalPosition; trajectoryInfoMap[joint] = null; } else { trajectoryInfo.CurrentTime = Time.time; if (goalPosition > trajectoryInfo.CurrentPosition) { trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition + movingDistance; } else { trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition - movingDistance; } if (PR2Common.continuousJoints.Contains(joint)) { trajectoryInfo.CurrentPosition = GetNormalizedRadian(trajectoryInfo.CurrentPosition); } newPosition = trajectoryInfo.CurrentPosition; } return(newPosition); }
private void UpdateLinkAngle(Transform link, PR2Common.Joint joint, Vector3 axis) { float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint) * Mathf.Rad2Deg; link.localEulerAngles = newPos * axis; }