public com.robotraconteur.geometry.Pose ToRRInfo()
{
var p = new com.robotraconteur.geometry.Pose();
CopyTo(ref p);
return(p);
}
public void GetState(out long last_recv, out bool enabled, out bool ready, out double[] joint_pos, out com.robotraconteur.geometry.Pose tcp_pose)
{
lock (this)
{
last_recv = this.last_recv;
joint_pos = this.actual_joint_position;
tcp_pose = this.tcp_pose;
enabled = this.enabled;
ready = this.ready;
}
}
protected virtual void _on_joint_position(Wire <Dictionary <string, double> > .WireConnection c, Dictionary <string, double> value, TimeSpec ts)
{
if (value == null)
{
return;
}
var pos = new double[_joint_names.Length];
for (int i = 0; i < _joint_names.Length; i++)
{
pos[i] = value[_joint_names[i]];
}
com.robotraconteur.geometry.Pose[] ep_pose = new com.robotraconteur.geometry.Pose[rox_robots_no_limits.Length];
try
{
for (int i = 0; i < rox_robots_no_limits.Length; i++)
{
var joint_numbers = this._robot_info.chains[i].joint_numbers;
double[] pos1 = new double[joint_numbers.Length];
for (int j = 0; j < joint_numbers.Length; j++)
{
pos1[j] = pos[joint_numbers[j]];
}
var rox_ep_pose = GeneralRoboticsToolbox.Functions.Fwdkin(rox_robots_no_limits[i], pos1);
ep_pose[i] = RobotRaconteur.Companion.Converters.GeometryConverter.ToPose(rox_ep_pose);
}
}
catch (Exception e)
{
ep_pose = null;
}
lock (this)
{
_joint_position = pos;
_endpoint_pose = ep_pose;
_last_joint_state = _stopwatch.ElapsedMilliseconds;
_last_endpoint_state = _stopwatch.ElapsedMilliseconds;
}
}
public void CopyTo(ref com.robotraconteur.geometry.Pose pose)
{
pose.position = position?.ToRRInfo() ?? new com.robotraconteur.geometry.Point();
pose.orientation = orientation?.ToRRInfo() ?? new com.robotraconteur.geometry.Quaternion();
}