# UR5 Jogging using Jacobian

I am not sure if this is the right place for this question, but here it is :

Based on the suggestions I found in the forums I am trying to get UR5 to jog. I have set the type of

arm_controller :
type: velocity_controllers/JointTrajectoryController


So I am guessing, it needs only velocity commands. Here is the code till now :

const std::vector<std::string> joint_names = jmg->getActiveJointModelNames();
ros::Rate loop_rate(5);
while(ros::ok()) {
Eigen::MatrixXd jacobian = group->getCurrentState()->getJacobian(jmg);
std::cout << "jacobian : " << jacobian << std::endl;

Eigen::VectorXd ee_vel(6);
ee_vel << -1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
std::cout << "ee vel : " << ee_vel << std::endl;

Eigen::VectorXd joint_vel = jacobian.inverse() * ee_vel;
std::cout << "joint vel : " << joint_vel << std::endl;

control_msgs::FollowJointTrajectoryGoal goal;
goal.trajectory.joint_names = joint_names;

goal.trajectory.points.resize(1);
goal.trajectory.points[0].positions.resize(6);
std::vector<double> joint_vals = group->getCurrentJointValues();
for (int i = 0; i < 6; ++i) {
goal.trajectory.points[0].positions[i] = joint_vals[i];
}

goal.trajectory.points[0].velocities.resize(6);
for (size_t j = 0; j < 6; ++j) {
goal.trajectory.points[0].velocities[j] = joint_vel[j];
}
// To be reached 1 second after starting along the trajectory
goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
traj_client->sendGoal(goal);

loop_rate.sleep();
}


I get a message saying :

[ INFO] [1475846670.761351237]: Received new trajectory execution service request...
[ INFO] [1475846670.761755780]: on_goal
[ INFO] [1475846670.981847762]: Execution completed: SUCCEEDED
[ INFO] [1475846672.982605141]: on_goal


But then the ur_driver crashes during the first loop.

Even though UR5 driver is capable of streaming joint data, In ur_driver.cpp

URDriver::setSpeed(...)


since there's no controller to support that, I am sending a joint trajectory with only one point in it.

The first few messages on ´/follow_joint_trajectory/goal´ are :

header:
seq: 0
stamp:
secs: 1476096642
nsecs: 351799456
frame_id: ''
goal_id:
stamp:
secs: 1476096642
nsecs: 351800506
id:
goal:
trajectory:
seq: 0
stamp:
secs: 1476096642
nsecs: 351783927
frame_id: ''
joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
points:
-
positions: [-0.33116084734071904, -1.6651738325702112, -1.6492660681353968, 3.313994884490967, -1.9009583632098597, 1.5703166723251343]
velocities: [-0.7625009696124342, 2.0051739782259315, -2.213010444549463, 0.20772017660027928, -0.7625008941085881, -0.00035870155518285494]
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs:         0
path_tolerance: []
goal_tolerance: []
goal_time_tolerance:
secs: 0
nsecs:         0
---
seq: 1
stamp:
secs: 1476096642
nsecs: 551965302
frame_id: ''
goal_id:
stamp:
secs: 1476096642
nsecs: 551965571
id: /cont_replan-3-1476096642.551965571
goal:
trajectory:
seq: 0
stamp:
secs: 1476096642
nsecs: 551939357
frame_id: ''
joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
points:
-
positions: [-0.3311851660357874, -1.6651976744281214, -1.6492541472064417, 3.314030647277832, -1.9010065237628382, 1.5701848268508911]
velocities: [-0.7625342648492489, 2.0051424575369525, -2.2130284353503162, 0.2077758962011466, -0.7625341972159444, -0.0003395046112384696]
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs:         0
path_tolerance: []
goal_tolerance: []
goal_time_tolerance:
secs: 0
nsecs:         0
---


Is my approach correct or is there a better way to do it? And why does the driver crash?

Originally posted by ipa-hsd on ROS Answers with karma: 150 on 2016-10-10

Post score: 1

Comment by ipa-hsd on 2016-10-10:
Is it possible that since the velocities are not starting from 0's, the driver is not able to handle these velocities? In that case, how to generate a trapezoidal profile for jog mode?

Comment by power93 on 2018-03-05:
Hi, Is true that i can send sample by sample my trajectory using speedj? Is there an example in c++ of a path following sending sample each 125 Hz? Thanks in advance.

Comment by Dreamer on 2022-11-11:

Update: a new package that handles Jacobian jogging: jog_arm

You can use UR's python interface, like this (bypassing ros_control). The downside is, there won't be any collision checking. We used a 50Hz loop rate.

        // Compose a string that contains URScript velocity command
sprintf(cmd_, "speedl([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], 0.2, 0.1)\n", delta.getX(), delta.getY(), delta.getZ(), 0., 0., 0.);
ROS_INFO_STREAM(cmd_);

// Copy command string to ROS message string
urscriptString_.data = cmd_;

// Send the compliant motion command to the UR5
velPub_.publish(urscriptString_);


Our lab has also used the Jacobian method like you were trying to do, but only on a Motoman arm (never tried it with a UR).

Originally posted by AndyZe with karma: 2331 on 2016-10-10

This answer was ACCEPTED on the original site

Post score: 0

Comment by AndyZe on 2016-10-10:
Here's the publisher definition:

// Publisher for the velocity commands

Comment by ipa-hsd on 2016-10-10:
the UR modern driver does exactly this. In ur_realtime_communication.cpp, the setSpeed function calls sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",q0, q1, q2, q3, q4, q5, acc);

Comment by ipa-hsd on 2016-10-10:
Does the trajectory has to have at least 2 points? Which actually makes sense..

Comment by AndyZe on 2016-10-10:
We're just sending one velocity command at a time, which works fine. But if you are sending position commands, I'm not sure.

Comment by ipa-hsd on 2016-10-11:
No, as I mentioned, I am sending velocity commands

Comment by ipa-hsd on 2016-10-12:
The solution works for me right now. Thanks!

Comment by toluwajosh on 2018-01-25:
Hi, could you please recommend a resource or provide a link to how I can obtain the Jacobian for a Motoman robot arm. I have tried some other methods and they don't seem to work.

Comment by AndyZe on 2018-01-25:
@intelharsh 's approach should work:

Eigen::MatrixXd jacobian = group->getCurrentState()->getJacobian(jmg);
`

Is MoveIt! set up correctly? Like, can you plan a move in RViz?

There's also a new pkg for this: jog_arm

Comment by toluwajosh on 2018-01-25:
So, I'm trying to include this function in a node which works well with Rviz. After adding the code snippet and trying to make with catkin_make I get the error; 'undefined reference to robot_model_loader, etc. So I'm wondering what could I be doing wrong.

Comment by toluwajosh on 2018-01-25:
I'll check out the jog_arm. Thanks