Hi guys, so this is a pretty complex problem so here goes.

Basically I have a robot, and I need that robot to grab an item and bring it to a plate located on its body. You can try to visualize it like this. First, put your arms to your chest. Then extend them out until they are parallel to the ground. Then bring them back to your chest. Thats the basic movement I'm trying to achieve.The robot's frame is designed such a way that the box has to fit into this very small compartment in the robot, hence the area that the box will be contained in is very small, requiring reasonably large amounts of precision in movement of joints. I am currently using an effortcontroller/jointposition_controller to control the joints and publishing the desired position in radians via their rostopic commands and have come into several problems. I currently have 2 methods of bringing the box to the robot.

Method 1 : Specifying the most direct joint destinations. I'll try to explain this as best as I can using one of the joints as an example. Lets say the starting position is 0 radians. To go to the box, the joint has to rotate 1 radian. And then to bring it back, it has to rotate back to 0 radians. So the command sequence is like this

1st Command : Go to 0 radians

2nd Command : Go to 1 radians

3rd Command : Go to 0 radians

And so on for each of the joints. The robot is like our human arm, it has 3 joints. One at the shoulder, one at the elbow and one at the wrist, hence I have to specify the position of 6 joints all together, 3 for each arm.

The problem with this? Remember how I said the box that the robot grips has to fit in a very small compartment within the robot ? Well this method causes a problem when the robot is RETRIEVING the box from the shelf. The movement from 1 radian to 0 radian (using the above example), smashes the box into the corners of the small compartment. It also makes the box sway alot from the sudden movement and is not smooth and fluid. I've tried all methods to make this method work such as adjusting the PID gains, limiting the effort of the joints. Somehow lowering the joints is easier and faster than lifting them up. A possible theory I came up with is that the effort controllers impart a constant effort to the joint to reach its desired position. And when lowering, an additional amount of effort is added ON TOP OF the already exerted effort due to gravity, making the downward movement easier. This method in theory seems to be the best as it provides the sweetest and shortest route from start to finish, but its causing me alot of problems, resulting in me trying the next method

Method 2 Specifying the joint destinations incrementally instead of one shot directly giving its end destination. Meaning the above example would be something like this. It would be too long to list but lets take the first command as an example only, to move from 0 radians to 1 radians

1st Command : Go to 0 radians

2nd Command : Go to 0.1 radians

3rd Command : Go to 0.2 radians

4th Command : etc, etc

11th Command : Go to 1 radians

This solves the problem of the robot smashing into the compartment as it provides me with greater and more precise control over the joints. However, the remaining problems are still there. The box still sways around and as you can imagine, the movement is quite unrealistic and jerky and I have to issue so many commands to the box. It works, but it isn't the prettiest thing.

Now I'm sure you're wondering, why don't you just post a video and save yourself all this long typing. Reason being is that this is a company project and I can't post images of the company robot on public domain, hence am restricted to typing out my problems...

Would really appreciate any help and thanks for reading this far !