void checkButtons()
        {
            if (OVRInput.GetDown(OVRInput.RawButton.X))
            {
                print("resetting");
                // Premeptively reset it to a nice position
                JointPositions joints = new JointPositions();
                joints.shoulder  = -51.0F;
                joints.upper_arm = -12.7F;
                joints.forearm   = -109.44F;
                joints.wrist_1   = -57.85F;
                joints.wrist_2   = 38.8F;
                joints.wrist_3   = 0.0F;
                joints.gripper   = 0.0F;

                executeCommand(joints);

                // Block the arm from accepting any new joint commands for 2s
                DateTime foo = DateTime.Now;
                restartTime = ((DateTimeOffset)foo).ToUnixTimeSeconds();

                // create an actual reset request to the big controller in the sky
                // s.t it doesn't send any new requests or save any data etc, saves the trajectory etc
                RPYProprioState proprio = new RPYProprioState();
                proprio.pos_x = -0.4F;
                proprio.pos_y = 0.2F;
                proprio.pos_z = 0.0F;
                RPYState r = new RPYState(
                    proprio,
                    GetAchievedGoal() // randomize this up above if desired
                    );
                ros.Send("reset", r);
            }
        }
        // void resetArticulation(int idx, int axis, float value) {
        //     //you cant just reset the ransform becasue then the x drive fights against it
        //     Vector3 originalRotation = articulationChain[idx].transform.eulerAngles;
        //     Vector3 resetRotation = new Vector3(originalRotation.x, originalRotation.y, originalRotation.z);
        //     resetRotation[axis] = value;
        //     //articulationChain[idx].transform.eulerAngles = resetRotation; // turns out doesn't work - some conflict between reset and set xdrive?
        //     set_articulation_position(idx, value);
        // }

        // bool convergingToReset(ResetInfo state) {

        //     // Would be great to so something like this, but get properties doesnt seem to work
        //     // PropertyInfo[] properties = state.GetProperties();
        //     //     print(properties);
        //     //     foreach (PropertyInfo pi in properties)
        //     //     {
        //     //         print(string.Format("Name: {0} | Value: {1}", pi.Name, pi.GetValue(proprio, null)));
        //     //     }


        //     if ((Math.Abs(-articulationChain[1].transform.eulerAngles.y - state.shoulder_link) < threshold)&
        //     (Math.Abs(wrapEuler(articulationChain[2].transform.eulerAngles.x) -  state.upper_arm_link) < threshold)&
        //     (Math.Abs(wrapEuler(articulationChain[3].transform.eulerAngles.x) -  state.forearm_link) < threshold)&
        //     (Math.Abs(wrapEuler(articulationChain[4].transform.eulerAngles.x) -  state.wrist_1_link) < threshold)&
        //     (Math.Abs(wrapEuler(-articulationChain[5].transform.eulerAngles.y) -  state.wrist_2_link) < threshold)&
        //     (Math.Abs(wrapEuler(articulationChain[6].transform.eulerAngles.x) -  state.wrist_3_link) < threshold) &
        //     // (Math.Abs(articulationChain[11].transform.eulerAngles.z + left_outer_knuckle_offset - state.left_outer_knuckle) < threshold)&
        //     // (Math.Abs(articulationChain[13].transform.eulerAngles.z + left_inner_finger_offset -  state.left_inner_finger) < threshold)&
        //     // (Math.Abs(articulationChain[15].transform.eulerAngles.z + left_inner_knuckle_offset -  state.left_inner_knuckle) < threshold)&
        //     // (Math.Abs(articulationChain[16].transform.eulerAngles.z + right_outer_knuckle_offset -  state.right_outer_knuckle) < threshold)&
        //     // (Math.Abs(articulationChain[18].transform.eulerAngles.z + right_inner_finger_offset -  state.right_inner_finger) < threshold)&
        //     // (Math.Abs(articulationChain[20].transform.eulerAngles.z + right_inner_knuckle_offset -  state.right_inner_knuckle) < threshold)&
        //     (Math.Abs(obj1.transform.position.x -  state.obj1_pos_x) < threshold) &
        //     (Math.Abs(obj1.transform.position.y -  state.obj1_pos_y) < threshold) &
        //     (Math.Abs(obj1.transform.position.z -  state.obj1_pos_z) < threshold) &
        //     (Math.Abs(obj1.transform.rotation.x -  state.obj1_q1) < threshold) &
        //     (Math.Abs(obj1.transform.rotation.y -  state.obj1_q2) < threshold) &
        //     (Math.Abs(obj1.transform.rotation.z -  state.obj1_q3) < threshold) &
        //     (Math.Abs(obj1.transform.rotation.w -  state.obj1_q4) < threshold))
        //     {


        //         return false;
        //     } else {
        //         //print(wrapEuler(articulationChain[2].transform.eulerAngles.x));

        //         print(state.forearm_link);
        //         print(   (Math.Abs(wrapEuler(articulationChain[3].transform.eulerAngles.x) -  state.forearm_link) < threshold)   );
        //         // print(state.left_outer_knuckle);
        //         // print(articulationChain[11].transform.rotation.z);
        //         // print(articulationChain[11].transform.eulerAngles.z + left_outer_knuckle_offset);
        //         // print(((Math.Abs(articulationChain[11].transform.eulerAngles.z + left_outer_knuckle_offset - state.left_outer_knuckle) < threshold)));
        //         return false;
        //     }
        // }


        void reset_to_default()
        {
            JointPositions joints = new JointPositions();

            joints.shoulder  = -51.0F;
            joints.upper_arm = -12.7F;
            joints.forearm   = -109.44F;
            joints.wrist_1   = -57.85F;
            joints.wrist_2   = 38.8F;
            joints.wrist_3   = 0.0F;
            joints.gripper   = 0.0F;

            executeCommand(joints);

            resetEnvironment(GetAchievedGoal());
        }
        // ####################################    Setters ######################################
        // This is to test when joint positions are broadcast
        void broadcastJointPositions()
        {
            DateTime foo      = DateTime.Now;
            long     unixTime = ((DateTimeOffset)foo).ToUnixTimeSeconds();

            JointPositions cmd = new JointPositions(
                jointSliderVals[1], // 1 - shoulder
                jointSliderVals[2], // 2 - upper arm
                jointSliderVals[3], // 3 - forearm
                jointSliderVals[4], // 4 - wrist 1
                jointSliderVals[5], // 5 - wrist 2
                jointSliderVals[6], // 6 - wrist 3
                0.0F,               // for the gripper
                unixTime            // unix time so we can ignore commands which are sitting in ROS' pipes but from prev runs
                );

            // Finally send the message to server_endpoint.py running in ROS
            ros.Send("joint_commands", cmd);
        }
        // this subscriber is called when joint commands happen, which can be
        // because a VR controller has pinged the IK server, or the joint sliders have sent out a topic
        void executeCommand(JointPositions command)
        {
            DateTime foo      = DateTime.Now;
            long     unixTime = ((DateTimeOffset)foo).ToUnixTimeSeconds();

            if (unixTime > restartTime + 2)
            {
                //adding elements using collection-initializer syntax
                // command.shoulder = Math.Min(Math.Max(command.shoulder, -140F), 60F);
                // command.upper_arm = Math.Min(Math.Max(command.upper_arm, -80F), 90F);
                // command.forearm = Math.Min(Math.Max(command.forearm, -130F), -10F);
                var positions = new List <float> {
                    0.00F, command.shoulder, command.upper_arm, command.forearm, command.wrist_1, command.wrist_2, command.wrist_3
                };
                // include the base non joint through the 0F
                foreach (int idx in Range(0, positions.Count))
                {
                    set_articulation_position(idx, positions[idx]);
                }
                grip(command.gripper);
            }
        }
예제 #5
0
 public ResetInfo(JointPositions joints, AchievedGoal ag)
 {
     this.joints = joints;
     this.ag     = ag;
 }
예제 #6
0
 public ResetInfo()
 {
     this.joints = new JointPositions();
     this.ag     = new AchievedGoal();
 }
 public findIKResponse(JointPositions act)
 {
     this.act = act;
 }
 public findIKResponse()
 {
     this.act = new JointPositions();
 }