0
$\begingroup$

Rosanswers logo

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);
    goal.trajectory.header.stamp = ros::Time::now();
    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: 
    header: 
      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
---
header: 
  seq: 1
  stamp: 
    secs: 1476096642
    nsecs: 551965302
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1476096642
    nsecs: 551965571
  id: /cont_replan-3-1476096642.551965571
goal: 
  trajectory: 
    header: 
      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


Original comments

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:
Hi,I'm researching on admittance control of UR5, but I am confused about how to realize the velocity control of UR5? Your code shown in this page is of great helpful for me, but can you show more details about the header file which should be included?and what is the variable jmg?Thanks a lot.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Original comments

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

// Publisher for the velocity commands

velPub_ = n_.advertise<std_msgs::String>("/right_ur5_controller/right_ur5_URScript", 1);

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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.