protected internal void _endpoint_state_cb(EndpointState msg)
{
lock (this)
{
_last_endpoint_state = _stopwatch.ElapsedMilliseconds;
if (!msg.valid)
{
_endpoint_pose = null;
_endpoint_vel = null;
return;
}
var p = new Pose();
p.orientation.w = msg.pose.orientation.w;
p.orientation.x = msg.pose.orientation.x;
p.orientation.y = msg.pose.orientation.y;
p.orientation.z = msg.pose.orientation.z;
p.position.x = msg.pose.position.x;
p.position.y = msg.pose.position.y;
p.position.z = msg.pose.position.z;
var v = new SpatialVelocity();
v.angular.x = msg.twist.angular.x;
v.angular.y = msg.twist.angular.y;
v.angular.z = msg.twist.angular.z;
v.linear.x = msg.twist.linear.x;
v.linear.y = msg.twist.linear.y;
v.linear.z = msg.twist.linear.z;
_endpoint_pose = new Pose[] { p };
_endpoint_vel = new SpatialVelocity[] { v };
}
}
protected internal void _endpoint_state_cb(EndpointState msg, int kin_chain)
{
lock (this)
{
_last_endpoint_state = _stopwatch.ElapsedMilliseconds;
/*if (!msg.valid)
* {
* _endpoint_pose = null;
* _endpoint_vel = null;
* return;
* }*/
var p = new Pose();
p.orientation.w = msg.pose.orientation.w;
p.orientation.x = msg.pose.orientation.x;
p.orientation.y = msg.pose.orientation.y;
p.orientation.z = msg.pose.orientation.z;
p.position.x = msg.pose.position.x;
p.position.y = msg.pose.position.y;
p.position.z = msg.pose.position.z;
var v = new SpatialVelocity();
v.angular.x = msg.twist.angular.x;
v.angular.y = msg.twist.angular.y;
v.angular.z = msg.twist.angular.z;
v.linear.x = msg.twist.linear.x;
v.linear.y = msg.twist.linear.y;
v.linear.z = msg.twist.linear.z;
if (_endpoint_pose == null)
{
if (_arm_selection == BaxterRobotArmSelection.both)
{
_endpoint_pose = new Pose[2];
}
else
{
_endpoint_pose = new Pose[1];
}
_endpoint_pose[kin_chain] = p;
}
if (_endpoint_vel == null)
{
_endpoint_vel = new SpatialVelocity[2];
_endpoint_vel[kin_chain] = v;
}
}
}
public static Vector <double> ToVector(SpatialVelocity v)
{
return(Vector <double> .Build.DenseOfArray(new double[] { v.angular.x, v.angular.y, v.angular.z, v.linear.x, v.linear.y, v.linear.z }));
}