protected void FixedUpdate()
{
if (this.gripperCommand == null)
{
return;
}
float newPos = this.GetGripperNewPosition();
if (Mathf.Abs(newPos - this.gripperCurrentPos) < 0.0001f) // Movement is small
{
this.gripperCommand = null;
return;
}
// Grasping and hand closing
if (this.graspedObject != null && newPos < this.gripperCurrentPos)
{
// Have to stop
this.gripperCommand = null;
return;
}
// Otherwise
else
{
float angleDiff = (newPos - this.gripperCurrentPos) * 0.52f / 0.086f * Mathf.Rad2Deg; // 0.52[rad](30[deg]) is max angle of a gripper joint and 0.086[m] is gripper distance.
this.gripperLFingerLink.localRotation *= Quaternion.Euler(0, 0, -angleDiff);
this.gripperRFingerLink.localRotation *= Quaternion.Euler(0, 0, -angleDiff);
this.gripperLFingerTipLink.localRotation *= Quaternion.Euler(0, 0, +angleDiff);
this.gripperRFingerTipLink.localRotation *= Quaternion.Euler(0, 0, +angleDiff);
}
this.gripperCurrentPos = newPos;
}
//protected override void Start()
//{
// base.Start();
//}
protected override void SubscribeMessageCallback(SIGVerse.RosBridge.pr2_controllers_msgs.Pr2GripperCommand gripperCommand)
{
this.gripperCommand = gripperCommand;
}
