Closed Kinematic control of Excavator Boom

edit

Attached is a gazebo model of an excavator .C:\fakepath\BoomJoint.png I am trying to control the boom angle by setting displacement position or velocity for the 2 cylinder-rods sitting on either side of the boom. The 3 revolute joints must then adjust ( boom to upper-house ) and cylinder to upper house ( 2 joints) according to the rod displacement/velocity to move the boom . I am not able to control this with position or velocity control from the joint GUI. With force control the joint move but break at a certain point. What is the optimal way to model this and control this? Should I eliminate the cylinder rod mechanism and just control the revolute joints and use a look up table for the displacement? Or should I reduce the joint stiffness on the 3 revolute joints. Would programmatic control work better than the GUI as it appears that in the GUI 2 commands are fighting each other? (No real improvement with programmatic control !)

I am now trying to adjust the damping and other parameters on the 3 revolute joints to be able to get some compliance on the joints. This appears to be a slow and cumbersome process . Has anyone looked into an automated process of parallel joint tuning ?

I have tried programmatic control with extra damping on the hinge joints. This either seems to be not possible or too difficult to tune !!