Where to Modify JointTrajectoryController?

Hello ROS Community!

(I assume what i am asking isnt the right way to solve things, and i am also sure that there are much better ways to solve the problem.)

My 5DOF system is controlled with the moveit commander, and the desired poses are successfully calculated (i.e. with group.set_pose_target and traj = group.plan).

For the real execution position_controllers/JointTrajectoryController is defined (ros_controllers).

Before the trajectory execution on the real robot, i want to manipulate the joint commands for the last two joints. Namely, i want to implement/modify the cpp (source of ros_controllers) somewhere in the lower level with the following:

joint_4 = joint_5calculated - joint_4calculated

joint_5 = -joint_5calculated - joint_4calculated,

where joint_icalculated denotes the calculated joint commands with IK.

Can someone recommend which cpp file would be the most suitable for this modification?

(The joint_state_controller contains the "inverse" of the aforementioned equations. This was quite easy to find and modify.)

edit retag close merge delete