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();
}
