protected override void _send_robot_command(long now, double[] joint_pos_cmd, double[] joint_vel_cmd) { if (joint_pos_cmd != null) { var msg = new JointCommand(); msg.header = new Header(); msg.mode = 1; msg.names = _joint_names; msg.position = joint_pos_cmd; _joint_command_pub.publish(msg); _position_command = joint_pos_cmd; _velocity_command = null; return; } if (joint_vel_cmd != null) { var msg = new JointCommand(); msg.header = new Header(); msg.mode = 2; msg.names = _joint_names; msg.velocity = joint_vel_cmd; _joint_command_pub.publish(msg); _position_command = null; _velocity_command = joint_vel_cmd; return; } }
public void Move(MotionCommand motion) { currentError = null; isMoving = true; LinearCommand lin = motion as LinearCommand; if (lin != null) { i = CalculateI(q); Position current = CurrentPosition(q); currentInterpolation = new LinearInterpolation( current, lin.target.AsPosition(current), lin.velocity * 1000.0f, i ); } JointCommand jnt = motion as JointCommand; if (jnt != null) { try { float[] target = jnt.target.AsArray(q); VerifySolution(target); currentInterpolation = new JointInterpolation( q, jnt.target.AsArray(q), jnt.velocity ); } catch (Exception ex) { currentError = ex.Message; finishedWithError = true; } } PtpCommand ptp = motion as PtpCommand; if (ptp != null) { try { i = CalculateI(q); float[] sol = Solve(ptp.target.AsPosition( CurrentPosition(q)), ptp.target.i0(i[0]), ptp.target.i1(i[1]), ptp.target.i2(i[2]) ).AsArray(); currentInterpolation = new JointInterpolation( q, sol, ptp.velocity ); } catch (Exception ex) { currentError = ex.Message; finishedWithError = true; } } }
protected override void _send_robot_command(long now, double[] joint_pos_cmd, double[] joint_vel_cmd) { if (joint_pos_cmd != null) { switch (_arm_selection) { case BaxterRobotArmSelection.left: { var msg_left = new JointCommand(); msg_left.mode = 1; msg_left.names = _joint_names_left; msg_left.command = joint_pos_cmd; _joint_command_pub_left.publish(msg_left); return; } case BaxterRobotArmSelection.right: { var msg_right = new JointCommand(); msg_right.mode = 1; msg_right.names = _joint_names_right; msg_right.command = joint_pos_cmd; _joint_command_pub_right.publish(msg_right); return; } case BaxterRobotArmSelection.both: { var msg_left = new JointCommand(); msg_left.mode = 1; msg_left.names = _joint_names_left; var left_pos_cmd = new double[7]; for (int i = 0; i < 7; i++) { left_pos_cmd[i] = joint_pos_cmd[i]; } msg_left.command = left_pos_cmd; _joint_command_pub_left.publish(msg_left); var msg_right = new JointCommand(); msg_right.mode = 1; msg_right.names = _joint_names_right; var right_pos_cmd = new double[7]; for (int i = 0; i < 7; i++) { right_pos_cmd[i] = joint_pos_cmd[i + 7]; } msg_right.command = right_pos_cmd; _joint_command_pub_right.publish(msg_right); return; } default: break; } } if (joint_vel_cmd != null) { switch (_arm_selection) { case BaxterRobotArmSelection.left: { var msg_left = new JointCommand(); msg_left.mode = 2; msg_left.names = _joint_names_left; msg_left.command = joint_vel_cmd; _joint_command_pub_left.publish(msg_left); return; } case BaxterRobotArmSelection.right: { var msg_right = new JointCommand(); msg_right.mode = 2; msg_right.names = _joint_names_right; msg_right.command = joint_vel_cmd; _joint_command_pub_right.publish(msg_right); return; } case BaxterRobotArmSelection.both: { var msg_left = new JointCommand(); msg_left.mode = 2; msg_left.names = _joint_names_left; var left_vel_cmd = new double[7]; for (int i = 0; i < 7; i++) { left_vel_cmd[i] = joint_vel_cmd[i]; } msg_left.command = left_vel_cmd; _joint_command_pub_left.publish(msg_left); var msg_right = new JointCommand(); msg_right.mode = 2; msg_right.names = _joint_names_right; var right_vel_cmd = new double[7]; for (int i = 0; i < 7; i++) { right_vel_cmd[i] = joint_vel_cmd[i + 7]; } msg_right.command = right_vel_cmd; _joint_command_pub_right.publish(msg_right); return; } } } }