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