void Awake()
{
List <UnityEngine.Transform> localLinkList = PR2Common.GetLinksInChildren(this.transform.root);
foreach (UnityEngine.Transform localLink in localLinkList)
{
if (localLink.name == PR2Common.Link.base_footprint.ToString())
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = PR2Common.OdomName;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
else
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = localLink.parent.name;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
}
this.synchronizer = this.GetComponent <PR2PubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
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; }
}
}
}
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>()
{
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // 01--12
this.torsoLiftLink.localPosition.z - this.torsoLiftLinkIniPosZ, // 13
0.0, // 14
PR2Common.GetRosAngleRad(PR2Common.Joint.head_pan_joint, Vector3.back, this.headPanLink), // 15
PR2Common.GetRosAngleRad(PR2Common.Joint.head_tilt_joint, Vector3.down, this.headTiltLink), // 16
0.0, // 17
PR2Common.GetRosAngleRad(PR2Common.Joint.r_upper_arm_roll_joint, Vector3.right, this.rUpperArmRollLink), // 18
PR2Common.GetRosAngleRad(PR2Common.Joint.r_shoulder_pan_joint, Vector3.back, this.rShoulderPanLink), // 19
PR2Common.GetRosAngleRad(PR2Common.Joint.r_shoulder_lift_joint, Vector3.down, this.rShoulderLiftLink), // 20
PR2Common.GetRosAngleRad(PR2Common.Joint.r_forearm_roll_joint, Vector3.right, this.rForearmRollLink), // 21
PR2Common.GetRosAngleRad(PR2Common.Joint.r_elbow_flex_joint, Vector3.down, this.rElbowFlexLink), // 22
PR2Common.GetRosAngleRad(PR2Common.Joint.r_wrist_flex_joint, Vector3.down, this.rWristFlexLink), // 23
PR2Common.GetRosAngleRad(PR2Common.Joint.r_wrist_roll_joint, Vector3.right, this.rWristRollLink), // 24
PR2Common.GetRosAngleRad(PR2Common.Joint.r_gripper_joint, Vector3.down, this.rGripperPalmLink), // 25
PR2Common.GetRosAngleRad(PR2Common.Joint.r_gripper_l_finger_joint, Vector3.back, this.rGripperLFingerLink), // 26
PR2Common.GetRosAngleRad(PR2Common.Joint.r_gripper_r_finger_joint, Vector3.back, this.rGripperRFingerLink), // 27
PR2Common.GetRosAngleRad(PR2Common.Joint.r_gripper_r_finger_tip_joint, Vector3.back, this.rGripperRFingerTipLink), // 28
PR2Common.GetRosAngleRad(PR2Common.Joint.r_gripper_l_finger_tip_joint, Vector3.back, this.rGripperLFingerTipLink), // 29
0.0, // 30
0.0, // 31
PR2Common.GetRosAngleRad(PR2Common.Joint.l_upper_arm_roll_joint, Vector3.right, this.lUpperArmRollLink), // 32
PR2Common.GetRosAngleRad(PR2Common.Joint.l_shoulder_pan_joint, Vector3.back, this.lShoulderPanLink), // 33
PR2Common.GetRosAngleRad(PR2Common.Joint.l_shoulder_lift_joint, Vector3.down, this.lShoulderLiftLink), // 34
PR2Common.GetRosAngleRad(PR2Common.Joint.l_forearm_roll_joint, Vector3.right, this.lForearmRollLink), // 35
PR2Common.GetRosAngleRad(PR2Common.Joint.l_elbow_flex_joint, Vector3.down, this.lElbowFlexLink), // 36
PR2Common.GetRosAngleRad(PR2Common.Joint.l_wrist_flex_joint, Vector3.down, this.lWristFlexLink), // 37
PR2Common.GetRosAngleRad(PR2Common.Joint.l_wrist_roll_joint, Vector3.right, this.lWristRollLink), // 38
PR2Common.GetRosAngleRad(PR2Common.Joint.l_gripper_joint, Vector3.down, this.lGripperPalmLink), // 39
PR2Common.GetRosAngleRad(PR2Common.Joint.l_gripper_l_finger_joint, Vector3.back, this.lGripperLFingerLink), // 40
PR2Common.GetRosAngleRad(PR2Common.Joint.l_gripper_r_finger_joint, Vector3.back, this.lGripperRFingerLink), // 41
PR2Common.GetRosAngleRad(PR2Common.Joint.l_gripper_r_finger_tip_joint, Vector3.back, this.lGripperRFingerTipLink), // 42
PR2Common.GetRosAngleRad(PR2Common.Joint.l_gripper_l_finger_tip_joint, Vector3.back, this.lGripperLFingerTipLink), // 43
0.0, // 44
0.0, // 45
};
this.UpdateContinuousJointVal(ref positions);
this.jointState.header.Update();
this.jointState.position = positions;
this.publisher.Publish(this.jointState);
}
private static bool IsOverLimitSpeed(PR2Common.Joint joint, double speed)
{
return(speed > PR2Common.GetMaxJointSpeed(joint));
}
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);
}
