private void updateModeAction() {
if (Input.GetButtonUp("Weapon")) {
hasWeapon = !hasWeapon;
}
isAttack = hasWeapon && Input.GetButton("Fire1");
if (hasWeapon) {
if (isAttack) {
armPose = ArmPose.None;
}
else {
armPose = ArmPose.HandleWeapon;
}
}
else {
armPose = ArmPose.None;
}
// 设置攻击状态
myAnimator.SetBool("isAttack",isAttack);
// 设置武器的显示
weapon.SetActive(hasWeapon);
// 设置手臂姿态
myAnimator.SetInteger("ArmPose",(int)armPose);
}
protected void updatePoseInfo()
{
ArmPose p = pose;
Vector3 dir = p.direction;
Vector3 pos = p.position;
Vector3 hUp = p.handUp;
infoForLabels.dir = "<" + dir.X.ToString("0.00") + ", " + dir.Y.ToString("0.00") + ", " + dir.Z.ToString("0.00") + ">";
infoForLabels.pos = "<" + pos.X.ToString("0.00") + ", " + pos.Y.ToString("0.00") + ", " + pos.Z.ToString("0.00") + ">";
infoForLabels.hUp = "<" + hUp.X.ToString("0.00") + ", " + hUp.Y.ToString("0.00") + ", " + hUp.Z.ToString("0.00") + ">";
}
public void SetArmPose(ArmPose pose)
{
SetDrive(worldJoint, pose.world_joint.angle);
SetDrive(baseJoint, pose.base_joint.angle);
SetDrive(shoulderJoint, pose.shoulder_joint.angle);
SetDrive(elbowJoint, pose.elbow_joint.angle);
SetDrive(wristJoint, pose.wrist_joint.angle);
SetDrive(effJoint, pose.eff_joint.angle);
SetDrive(rightGripper, pose.right_gripper_joint.angle);
SetDrive(leftGripper, pose.left_gripper_joint.angle);
}
void testPose()
{
printName("Pose");
ArmPose p = new ArmPose();
p.position = Vector3.UnitX;
p.direction = Vector3.UnitY;
p.handUp = Vector3.UnitZ;
testJSON(p);
}
void testKeyFrame()
{
printName("KeyFrame");
ArmPose p = new ArmPose();
p.position = Vector3.UnitX;
p.direction = Vector3.UnitY;
p.handUp = Vector3.UnitZ;
KeyFrame k = new KeyFrame(p, 0.444f);
testJSON(k);
}
public void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed < publishMessageFrequency)
{
return;
}
ArmPose armPose = arm.GetArmPose();
ros.Send(topicName, armPose);
timeElapsed = 0;
}
public void Invoke()
{
ArmPose pose = arm.GetArmPose();
JointState[] joints = new[]
{
pose.world_joint,
pose.base_joint,
pose.shoulder_joint,
pose.elbow_joint,
pose.wrist_joint,
pose.eff_joint,
pose.gripper_offset_joint
};
ForwardKinematicsRequest req = new ForwardKinematicsRequest(joints);
ros.SendServiceMessage <ForwardKinematicsResponse>("forward_kinematics", req, Callback);
}
public Arm6Axis()
{
pose = new ArmPose();
pose.direction = Vector3.UnitX;
pose.handUp = Vector3.UnitZ;
targetAngles = new Angle[6];
servoAngles = new Angle[6];
for (int i = 0; i < targetAngles.Length; i++)
{
targetAngles[i] = new Angle();
servoAngles[i] = new Angle();
}
workThread = new Thread(stepperLoop);
workThread.Start();
Thread.Sleep(Timeout.Infinite);
}
public void Invoke()
{
ArmPose pose = arm.GetArmPose();
var pos = arm.worldJoint.transform.InverseTransformPoint(target.transform.position);
Debug.Log(pos);
TrajectoryPlannerRequest req = new TrajectoryPlannerRequest(
pose,
new RosMessageTypes.Geometry.Pose(
new RosMessageTypes.Geometry.Point(
pos.x,
pos.y,
pos.z),
new RosMessageTypes.Geometry.Quaternion(0, 0, 0, 0)),
true,
false);
ros.SendServiceMessage <TrajectoryPlannerResponse>("trajectory_planner", req, Callback);
}
public ArmPose GetArmPose()
{
Vector3 offsetPos = gripperOffset.transform.localPosition;
JointState gripperOffsetJoint = new JointState();
gripperOffsetJoint.translation = new Point(offsetPos.x, offsetPos.y, offsetPos.z);
ArmPose armPose = new ArmPose(
GetJointState(worldJoint, 0),
GetJointState(baseJoint, 1),
GetJointState(shoulderJoint, 3),
GetJointState(elbowJoint, 3),
GetJointState(wristJoint, 3),
GetJointState(effJoint, 1),
gripperOffsetJoint,
GetJointState(rightGripper, 3),
GetJointState(leftGripper, 3));
return armPose;
}
private void updateRecieved(ComData data)
{
if (data.isType <ArmPose>())
{
pose = (ArmPose)data.value[0];
updatePoseInfo();
workerResumeEvent.Set();
}
else if (data.isType <Angle>())
{
servoAngles = new Angle[data.value.Length];
for (int i = 0; i < data.value.Length; i++)
{
servoAngles[i] = (Angle)data.value[i];
}
updateAngleInfo();
}
updateLabelsDelegate(infoForLabels);
}
public void Invoke()
{
ArmPose arm_pose = arm.GetArmPose();
Vector3 wPos = WristTarget.transform.localPosition;
Quaternion wRot = WristTarget.transform.localRotation;
Vector3 ePos = arm.worldJoint.transform.InverseTransformPoint(EffTarget.transform.position);
Quaternion eRot = EffTarget.transform.localRotation;
InverseKinematicsRequest req = new InverseKinematicsRequest(
arm_pose,
new RosMessageTypes.Geometry.Pose(
new RosMessageTypes.Geometry.Point(
wPos.x, wPos.y, wPos.z),
new RosMessageTypes.Geometry.Quaternion(
wRot.x, wRot.y, wRot.z, wRot.w)),
new RosMessageTypes.Geometry.Pose(
new RosMessageTypes.Geometry.Point(
ePos.x, ePos.y, ePos.z),
new RosMessageTypes.Geometry.Quaternion(
eRot.x, eRot.y, eRot.z, eRot.w)));
ros.SendServiceMessage <InverseKinematicsResponse>("inverse_kinematics", req, Callback);
}
public override bool getNextTarget(ref ComData d, Collection <Key> keysDown, Collection <Key> keysToggle, ArmPose arm, float stepAmount)
{
stepAmount *= CRot;
if (keysDown.Contains(Key.LeftShift) || keysDown.Contains(Key.RightShift))
{
stepAmount *= -1;
}
Key[] toCheck =
{
//Key.D0,
Key.D1, Key.D2,
Key.D3, Key.D4,
Key.D5, Key.D6
};
Angle[] dAngles = new Angle[toCheck.Length];
bool changed = false;
for (int i = 0; i < toCheck.Length; i++)
{
Key k = toCheck[i];
if (keysDown.Contains(k))
{
dAngles[i] = stepAmount;
changed = true;
}
else
{
dAngles[i] = 0;
}
}
//Console.WriteLine("raw");
d = changed ? new ComData(dAngles) : d;
return(changed);
}
public override bool getNextTarget(ref ComData d, Collection <Key> keysDown, Collection <Key> keysToggle, ArmPose pose, float stepAmount)
{
Vector3 dPos = new Vector3();
Vector3 dDir = new Vector3(); // just for storing dRotation angles
if (keysDown.Contains(Key.A))
{
dPos.X += -1;
}
if (keysDown.Contains(Key.D))
{
dPos.X += 1;
}
if (keysDown.Contains(Key.S))
{
dPos.Y += -1;
}
if (keysDown.Contains(Key.W))
{
dPos.Y += 1;
}
if (keysDown.Contains(Key.C))
{
dPos.Z += -1;
}
if (keysDown.Contains(Key.Z))
{
dPos.Z += 1;
}
if (keysDown.Contains(Key.J))
{
dDir.Y += -1;
}
if (keysDown.Contains(Key.L))
{
dDir.Y += 1;
}
if (keysDown.Contains(Key.I))
{
dDir.X += -1;
}
if (keysDown.Contains(Key.K))
{
dDir.X += 1;
}
if (keysDown.Contains(Key.M))
{
dDir.Z += -1;
}
if (keysDown.Contains(Key.OemPeriod))
{
dDir.Z += 1;
}
if (dPos == Vector3.Zero && dDir == Vector3.Zero)
{
return(false);
}
dPos = dPos.LengthSquared() != 0? Vector3.Normalize(dPos) * stepAmount * CTrans : dPos;
dDir = dDir.LengthSquared() != 0? Vector3.Normalize(dDir) * stepAmount * CRot : dDir;
var dPose = new ArmPose(dPos, dDir, dDir);
d = new ComData(new KeyFrame(dPose, stepAmount));
//Console.WriteLine(dDir.ToString());
return(true);
}
/// <summary>Update this arm, using state from the appropriate joints of this body.</summary>
/// <returns>The HandTransition that just happened, if in fact any did.</returns>
public Option<HandPose> Update(HolofunkBody holofunkBody, Body body)
{
// first calculate the arm pose
JointType hand = m_side == Side.Left ? JointType.HandLeft : JointType.HandRight;
Vector2 handToHead = holofunkBody[hand] - holofunkBody[JointType.Head];
handToHead.X = (float)Math.Round(handToHead.X);
handToHead.Y = (float)Math.Round(handToHead.Y);
Vector2 headToUpperChest = holofunkBody[JointType.Head] - holofunkBody[JointType.SpineShoulder];
Vector2 handToUpperChest = holofunkBody[hand] - holofunkBody[JointType.SpineShoulder];
Vector2 handToChest = holofunkBody[hand] - holofunkBody[JointType.SpineMid];
Vector2 headToChest = holofunkBody[JointType.SpineMid] - holofunkBody[JointType.Head];
/*
string topLine = null;
if (m_side == Side.Left) {
// B4CR: TONS OF GARBAGE: TEMP ONLY
topLine = "";
topLine = topLine + "\nhandToHead " + handToHead + ", handToHead.L " + handToHead.Length();
topLine = topLine + "\nheadToUpperChest " + headToUpperChest + ", headToUpperChest.L " + headToUpperChest.Length();
topLine = topLine + "\nhandToChest " + handToChest + ", handToChest.L " + handToChest.Length();
topLine = topLine + "\nchestToUpperChest " + chestToUpperChest + ", chestToUpperChest.L " + chestToUpperChest.Length();
}
if (topLine != null) {
Spam.TopLine2 = topLine;
}
*/
if (holofunkBody[hand].X > holofunkBody[JointType.ShoulderLeft].X
&& holofunkBody[hand].X < holofunkBody[JointType.ShoulderRight].X
&& handToHead.Y < 0) {
m_pose = ArmPose.OnHead;
}
else if (handToHead.LengthSquared() < headToUpperChest.LengthSquared()
&& handToHead.LengthSquared() < handToChest.LengthSquared()) {
m_pose = ArmPose.AtMouth;
}
else if (handToUpperChest.LengthSquared() < headToUpperChest.LengthSquared() * 1.5f) {
m_pose = ArmPose.AtChest;
}
else {
m_pose = ArmPose.Unknown;
}
// then calculate the old & new (smoothed) hand states, and fire event if it changed
HandState handState = m_side == Side.Left ? body.HandLeftState : body.HandRightState;
HandState priorTrackerState = m_handTracker.HandState;
m_handTracker.Update(handState);
HandState postTrackerState = m_handTracker.HandState;
if (priorTrackerState != postTrackerState) {
switch (postTrackerState) {
case HandState.Open:
return HandPose.Opened;
case HandState.Closed:
return HandPose.Closed;
case HandState.Lasso:
return HandPose.Pointing;
case HandState.NotTracked:
case HandState.Unknown:
return HandPose.Unknown;
}
}
return Option<HandPose>.None;
}
public void OnLeftArm(ArmPose transition)
{
Moment now = HolofunkModel.Clock.Now;
// fire Other* event at the *other* hand model
lock (m_rightHandModel.StateMachine) {
Spam.Model.WriteLine("PlayerModel.OnLeftArm: received transition " + transition);
m_rightHandModel.StateMachine.OnNext(LoopieEvent.FromArmPose(transition), now);
}
}
public void OnRightArm(ArmPose transition)
{
Moment now = HolofunkModel.Clock.Now;
lock (m_leftHandModel.StateMachine) {
Spam.Model.WriteLine("PlayerModel.OnRightArm: received transition " + transition);
m_leftHandModel.StateMachine.OnNext(LoopieEvent.FromArmPose(transition), now);
}
}
public abstract bool getNextTarget(ref ComData d, Collection <Key> keysDown, Collection <Key> keysToggle, ArmPose pose, float stepAmount);
