Exemple #1
0
        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;
                }
            }
        }
Exemple #3
0
 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 }));
 }