Fix inverse kinematics to keep end effector straight #1
Description
Detailed Description
The end effector needs to be kept straight when moving in order to properly attach to blocks.
Expected Behavior
The simulation should be updated so that when the robot moves, the two end effectors are kept straight to insert into the blocks.
Current Behavior
This feature is not currently implemented, and the robot moves the end effector to all kinds of new angles.
Ideas on Resolution
- Look into imposing constraints on the inverse kinematics.