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); } }
public ResetInfo(JointPositions joints, AchievedGoal ag) { this.joints = joints; this.ag = ag; }
public ResetInfo() { this.joints = new JointPositions(); this.ag = new AchievedGoal(); }
public findIKResponse(JointPositions act) { this.act = act; }
public findIKResponse() { this.act = new JointPositions(); }