public void SetJointTargetVelocityHandler(armproxy.SetJointTargetVelocity update) { /* update.Body.JointName; update.Body.TargetVelocity; */ }
public void ArmReliableSubscribeHandler(armproxy.ReliableSubscribe reliablesubscribe) { throw new NotImplementedException(); }
public void GetEndEffectorPoseHandler(armproxy.GetEndEffectorPose getendeffectorpose) { throw new NotImplementedException(); }
public void SetJointTargetPoseHandler(armproxy.SetJointTargetPose update) { string name = update.Body.JointName; JointState j = _jointLookup[name]; /* update.Body.TargetOrientation; update.Body.TargetPosition; */ }
public void SetEndEffectorPoseHandler(armproxy.SetEndEffectorPose update) { Pose pose = update.Body.EndEffectorPose; Quaternion or = pose.Orientation; Vector3 pos = pose.Position; }
public IEnumerator<ITask> ArmGetHandler(armproxy.Get get) { yield return Arbiter.Choice( UpdateState(), success => get.ResponsePort.Post((armproxy.ArticulatedArmState) (_state.Clone())), ex => get.ResponsePort.Post(Fault.FromException(ex)) ); }
public void ArmSubscribeHandler(armproxy.Subscribe subscribe) { SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort); }
/// <summary> /// Set End Effector /// </summary> /// <param name="update"></param> /// <returns></returns> /// <remarks>Moves the arm end effector to a specified pose</remarks> private IEnumerator<ITask> SetEndEffectorHandler(armproxy.SetEndEffectorPose update) { bool success = false; float baseAngle0, shoulder0, elbow0, wrist0, rotateVal0, gripperVal0; // Get the target joint angles for all joints // We have to do this so that we can preserve the values for the joints // that are NOT being changed. Note that we do not use the CURRENT joint // angles because this could potentially stop a motion in progress. _robotArm.GetTargetJointAngles(out baseAngle0, out shoulder0, out elbow0, out wrist0, out rotateVal0, out gripperVal0); // Get all the pose information required // NOTE: The angle is assumed to be about the X axis of the gripper Quaternion q = new Quaternion( update.Body.EndEffectorPose.Orientation.X, update.Body.EndEffectorPose.Orientation.Y, update.Body.EndEffectorPose.Orientation.Z, update.Body.EndEffectorPose.Orientation.W); AxisAngle a = Quaternion.ToAxisAngle(q); float p = Conversions.RadiansToDegrees(a.Angle * Math.Sign(a.Axis.X)); float x = update.Body.EndEffectorPose.Position.X; float y = update.Body.EndEffectorPose.Position.Y; float z = update.Body.EndEffectorPose.Position.Z; _moveTargetEntity.Position = new xna.Vector3(x, y, z); float baseAngle, shoulder, elbow, wrist; // Use Inverse Kinematics to calculate the joint angles if (!_kinematics.InverseKinematics(x, y, z, p, out baseAngle, out shoulder, out elbow, out wrist)) { // The results were not a valid pose! // Don't attempt the move or it might break the arm // Should put something in the Fault! Fault f = Fault.FromException(new Exception("No solution to Inverse Kinematics")); update.ResponsePort.Post(f); yield break; } // calculate the time needed to make the motion float maxAngle = Math.Max(Math.Abs(baseAngle0 - baseAngle), Math.Abs(shoulder0 - shoulder)); maxAngle = Math.Max(maxAngle, Math.Abs(elbow0 - elbow)); maxAngle = Math.Max(maxAngle, Math.Abs(wrist0 - wrist)); float time = maxAngle * 11f / 360f; // Set the arm if (time <= 0.1f) { time = 0.1f; } yield return Arbiter.Choice(_robotArm.MoveTo(baseAngle, shoulder, elbow, wrist, rotateVal0, gripperVal0, time), delegate { success = true; }, ShowError); if (success) update.ResponsePort.Post(DefaultUpdateResponseType.Instance); else { // Should put something in the Fault! Fault f = Fault.FromException(new Exception("MoveTo failed")); update.ResponsePort.Post(f); } // NOT FINISHED! // we send a replace notification since end effector pose updates affect all joints var replace = new armproxy.Replace {Body = _state}; base.SendNotification(_submgrPort, replace); // send an end effector update notification as well base.SendNotification(_submgrPort, update); yield break; }
public void ReplaceArticulatedArmJointList(arm.ArticulatedArmState state) { btnConnect_ArticulatedArm.Enabled = false; btnJointParamsApply.Enabled = true; listArticulatedArmJoints.BeginUpdate(); try { listArticulatedArmJoints.Tag = state; listArticulatedArmJoints.Items.Clear(); if (state.Joints.Count > 0) { UriBuilder node = new UriBuilder(state.Joints[0].State.Name); node.Path = null; lblActiveJointValue.Text = state.Joints[0].State.Name; foreach (Joint j in state.Joints) { listArticulatedArmJoints.Items.Add(j.State.Name); } } else { lblActiveJointValue.Text = string.Empty; } } finally { listArticulatedArmJoints.EndUpdate(); } }