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