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; }
}
}
}