modified const force script

added pid movement example to show how the robot can control a contact
force on tcp