ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# 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?

edit retag close merge delete

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?

( 2016-10-10 09:21:50 -0600 )edit

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.

( 2018-03-05 13:17:19 -0600 )edit

( 2022-11-11 01:10:31 -0600 )edit

Sort by » oldest newest most voted

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).

more

Here's the publisher definition:

// Publisher for the velocity commands

( 2016-10-10 09:57:59 -0600 )edit

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);

( 2016-10-10 09:58:31 -0600 )edit

Does the trajectory has to have at least 2 points? Which actually makes sense..

( 2016-10-10 10:09:57 -0600 )edit

We're just sending one velocity command at a time, which works fine. But if you are sending position commands, I'm not sure.

( 2016-10-10 10:21:20 -0600 )edit

No, as I mentioned, I am sending velocity commands

( 2016-10-11 04:24:49 -0600 )edit

The solution works for me right now. Thanks!

( 2016-10-12 07:13:32 -0600 )edit

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.

( 2018-01-25 04:23:08 -0600 )edit

@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

( 2018-01-25 09:01:06 -0600 )edit