public com.robotraconteur.geometry.SpatialVelocity ToRRInfo()
{
var info = new com.robotraconteur.geometry.SpatialVelocity();
CopyTo(ref info);
return(info);
}
protected virtual void _on_joint_velocity(Wire <Dictionary <string, double> > .WireConnection c, Dictionary <string, double> value, TimeSpec ts)
{
if (value == null)
{
return;
}
var vel = new double[_joint_names.Length];
for (int i = 0; i < _joint_names.Length; i++)
{
vel[i] = value[_joint_names[i]];
}
com.robotraconteur.geometry.SpatialVelocity[] ep_vel = new com.robotraconteur.geometry.SpatialVelocity[rox_robots_no_limits.Length];
try
{
for (int i = 0; i < _rox_robots.Length; i++)
{
var joint_numbers = this._robot_info.chains[i].joint_numbers;
double[] vel1 = new double[joint_numbers.Length];
for (int j = 0; j < joint_numbers.Length; j++)
{
vel1[j] = vel[joint_numbers[j]];
}
var rox_ep_jac = GeneralRoboticsToolbox.Functions.Robotjacobian(_rox_robots[i], vel1);
var rox_ep_vel = rox_ep_jac.Multiply(MathNet.Numerics.LinearAlgebra.Vector <double> .Build.DenseOfArray(vel));
ep_vel[i] = RobotRaconteur.Companion.Converters.GeometryConverter.ToSpatialVelocity(rox_ep_vel);
}
}
catch (Exception e)
{
ep_vel = null;
}
lock (this)
{
_endpoint_vel = ep_vel;
_joint_velocity = vel;
}
}
public void CopyTo(ref com.robotraconteur.geometry.SpatialVelocity info)
{
info.angular = angular?.ToRRInfo() ?? default;
info.linear = linear?.ToRRInfo() ?? default;
}