UR10 Motion Control with ROS 2 and MATLAB

I wanted to drive a simulated UR10 from MATLAB over ROS 2 end to end, and then check that the logged joint positions and velocities actually look the way the maths says they should.

Sending a manipulator to a target pose is more than one call. You need an inverse kinematics solution, a correctly filled trajectory message, and a way to see what the controller really did. This project builds that whole loop from MATLAB and compares the executed motion against the quintic-polynomial profile it was meant to follow.

The UR10 is imported as a RigidBodyTree (8 bodies, 6 revolute joints). For a target pose the numerical solver minimizes a weighted pose error, a nonlinear least-squares problem in the joint vector $\mathbf{q}$:

A damped least-squares (Levenberg-Marquardt) step with the Jacobian $\mathbf{J}=\partial\mathbf{e}/\partial\mathbf{q}$ is the standard update:

In my runs the solver used BFGS gradient projection with the seed [0 1 -2 2 1 1] for the target position [0.6, 0, 1.0] m and Euler angles [pi/4, pi/4, -pi/4]. The solver is local, so the seed selects among the valid configurations (for example elbow-up or elbow-down), and the solver status and the pose error are checked before the result is sent to the robot.

The joint trajectory controller tracks waypoints with a fifth-order polynomial. For a point-to-point move over $t_f$ the six coefficients are fixed by position and by zero velocity and acceleration at both ends:

Commands reach the controller by two mechanisms. The topic interface (trajectory_msgs/JointTrajectory) is fire-and-forget: execution is not monitored and tolerance violations are not reported. The action interface (control_msgs/FollowJointTrajectory) returns immediately from sendGoal, streams feedback (desired, actual and error per joint), reports a result, supports cancellation, and aborts the goal when path or goal tolerances are violated.

• Started the simulation with ros2 launch ur_simulation_gazebo ur_sim_control.launch.py ur_type:=ur10 gazebo_gui:=false and shifted the floor joint to -2 on Z so the arm does not clip the floor at spawn.
• Created /matlab_node with a subscriber on /joint_states and a publisher for the trajectory topic.
• Solved IK for the target pose and filled the JointTrajectory message (header stamp, 6 joint names, waypoints, velocities, times).
• Logged /joint_states at 50 Hz (150 samples for a 3.0 s window) with the message header stamps as time axis, because receive blocks and the loop index is not a clock.
• Switched to the FollowJointTrajectory action client with activation, feedback and result callbacks.
• Fixed the noisy first runs by raising the controller tolerances in urdf_controller.yaml.

• Point-to-point motion settles at the target by about 1.6 s.
• Peak joint velocity matches the quintic prediction: joint 3 about 1.5 rad/s predicted and measured; joint 4 about 1.65 rad/s predicted, about 1.6 rad/s measured.
• The velocity requested at the intermediate waypoint decides whether a multi-waypoint move is smooth or staggered.
• Looser controller tolerances remove the tracking-error aborts in simulation but can hide real tracking errors, so they need tightening on hardware.

• Compare joint-space quintic motion against Cartesian-space paths for the same end-effector targets.
• Check the solver's exit flag and pose error automatically before sending any IK result.
• Move the MATLAB side into a native ROS 2 node (rclcpp or rclpy) to drop the MATLAB dependency.