public FullState(QuaternionProprioState proprio, AchievedGoal ag, Sensor.Image shoulderImage, Sensor.Image gripperImage)
{
this.proprio = proprio;
this.ag = ag;
this.shoulderImage = shoulderImage;
this.gripperImage = gripperImage;
}
public FullState()
{
this.proprio = new QuaternionProprioState();
this.ag = new AchievedGoal();
this.shoulderImage = new Sensor.Image();
this.gripperImage = new Sensor.Image();
}
QuaternionProprioState GetQuaternionProprioState()
{
// arm information
Transform grasp_target = articulationChain[ee_index].transform;
Transform ee_info = articulationChain[ee_index - 1].transform;
//
QuaternionProprioState proprio = new QuaternionProprioState(
grasp_target.position.x, //start arm
grasp_target.position.y,
grasp_target.position.z,
ee_info.rotation.x,
ee_info.rotation.y,
ee_info.rotation.z,
ee_info.rotation.w,
(articulationChain[11].transform.localEulerAngles.z + left_outer_knuckle_offset) / 40.0F // gripper state from this
);
return(proprio);
}
public CheckPoseRequest(QuaternionProprioState QuatPos)
{
this.QuatPos = QuatPos;
}
public CheckPoseRequest()
{
this.QuatPos = new QuaternionProprioState();
}
