Ask Your Question
1

UR5 Jogging using Jacobian

asked 2016-10-10 06:27:09 -0500

intelharsh gravatar image

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?

edit retag flag offensive close merge delete

Comments

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?

intelharsh gravatar imageintelharsh ( 2016-10-10 09:21:50 -0500 )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.

power93 gravatar imagepower93 ( 2018-03-05 13:17:19 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2016-10-10 09:44:52 -0500

AndyZe gravatar image

updated 2018-01-25 09:07:28 -0500

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

edit flag offensive delete link more

Comments

Here's the publisher definition:

// Publisher for the velocity commands

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

AndyZe gravatar imageAndyZe ( 2016-10-10 09:57:59 -0500 )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);

intelharsh gravatar imageintelharsh ( 2016-10-10 09:58:31 -0500 )edit

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

intelharsh gravatar imageintelharsh ( 2016-10-10 10:09:57 -0500 )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.

AndyZe gravatar imageAndyZe ( 2016-10-10 10:21:20 -0500 )edit

No, as I mentioned, I am sending velocity commands

intelharsh gravatar imageintelharsh ( 2016-10-11 04:24:49 -0500 )edit

The solution works for me right now. Thanks!

intelharsh gravatar imageintelharsh ( 2016-10-12 07:13:32 -0500 )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.

toluwajosh gravatar imagetoluwajosh ( 2018-01-25 04:23:08 -0500 )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

AndyZe gravatar imageAndyZe ( 2018-01-25 09:01:06 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

4 followers

Stats

Asked: 2016-10-10 06:27:09 -0500

Seen: 971 times

Last updated: Jan 25 '18