[ROS2] Add wall in front of user for all movements but the final MoveToMouth / initial MoveFromMouth
amalnanavati opened this issue · comments
Currently, sometimes the MoveFromMouth action will have a cartesian motion back from the mouth, and will then move closer to the user's face during the kinematic motion. That movement closer to the user's face feels scary and unnecessary.
To address this, we should have a wall in front of the user that prevents the robot from moving closer to the user. For safety, we should have this wall for all motions, and only disable it for the cartesian motions to/from the mouth in MoveToMouth / MoveFromMouth. However, if we put this wall too close to the staging pose, it is possible that the end configuration of the cartesian motion in MoveFromMouth leaves the robot in collision with this. Therefore, we might also need to be able to move this wall based on the robot arm pose so it is always right beyond the forktip, and essentially prevents the robot from moving closer to the user than it already is.
Reopening because I saw a MoveToMouth movement to the staging configuration that moved unnecessarily close to the head. So even MoveToMouth should add this wall for the movement to the staging configuration.