Skip to content

Commit

Permalink
release
Browse files Browse the repository at this point in the history
  • Loading branch information
cfry committed Mar 3, 2021
1 parent d9ad677 commit ae345d8
Show file tree
Hide file tree
Showing 390 changed files with 1,247 additions and 18,260 deletions.
77 changes: 25 additions & 52 deletions core/dextersim.js
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,7 @@ DexterSim = class DexterSim{
this.robot_name = robot_name
this.robot = Robot[robot_name] //only used by predict_move_dur
DexterSim.robot_name_to_dextersim_instance_map[robot_name] = this

this.a_angles_arcseconds = [0,0,0,0,0,0,0]
this.measured_angles_arcseconds = [0,0,0,0,0,0,0] //a_angles + pid_angles
this.measured_angles_dexter_units = [0,0,0,0,0,0,0] //a_angles + pid_angles
}

//sim_actual passed in is either true or "both"
Expand Down Expand Up @@ -51,7 +49,7 @@ DexterSim = class DexterSim{
this.write_file_file_content = "" //grows as "m" instructions come in


this.pid_angles_arcseconds = [0,0,0,0,0,0,0]
// not used this.pid_angles_arcseconds = [0,0,0,0,0,0,0]
this.velocity_arcseconds_per_second = [0,0,0,0,0,0,0]

this.ready_to_start_new_instruction = true //true at beginning and when we've just completed an instruction but not started another
Expand Down Expand Up @@ -108,11 +106,11 @@ DexterSim = class DexterSim{

//called from Socket.send
//typically adds instruction to sim_inst.instruction_queue
static send(robot_name, arr_buff){ //instruction_array is in arcseconds
static send(robot_name, arr_buff){
//out("Sim.send passed instruction_array: " + instruction_array + " robot_name: " + robot_name)
let instruction_array = this.array_buffer_to_oplet_array(arr_buff)
let ins_args = Instruction.args(instruction_array) //in arcseconds
let sim_inst = DexterSim.robot_name_to_dextersim_instance_map[robot_name]
let instruction_array = this.array_buffer_to_oplet_array(arr_buff) //instruction_array is in dexter_units
let ins_args = Instruction.args(instruction_array) //in dexter_units
sim_inst.sent_instructions_count += 1
if (sim_inst.status == "closing"){
shouldnt("In a_DexterSim.send with robot_name: " + robot_name +
Expand Down Expand Up @@ -200,7 +198,7 @@ DexterSim = class DexterSim{
let robot_status_array = Dexter.make_default_status_array_g_sm(this.status_mode)
let rs_inst = new RobotStatus({robot_status: robot_status_array})
if(rs_inst.supports_measured_angles()) {
rs_inst.set_measured_angles(this.measured_angles_arcseconds, true) //we want to install arcseconds, as Societ is expected arcseconds and will convert to degrees
rs_inst.set_measured_angles(this.measured_angles_dexter_units, true) //we want to install arcseconds, as Socket is expected arcseconds and will convert to degrees
}
//else allow all other status modes.
robot_status_array[Dexter.JOB_ID] = instruction_array[Instruction.JOB_ID]
Expand Down Expand Up @@ -267,12 +265,6 @@ DexterSim = class DexterSim{
sim_inst.now_processing_instruction &&
(sim_inst.ending_time_of_cur_instruction <= the_now)) { //end the cur instruction and move to the next
const oplet = sim_inst.now_processing_instruction[Dexter.INSTRUCTION_TYPE]
//if ((sim_inst.sim_actual === true) && [/*"F" , "G", "g"*/].includes(oplet)) { //dont do when sim == "both"
//let rs_copy = sim_inst.robot_status_in_arcseconds.slice() //make a copy to return as some subseqent call to this meth will modify the one "model of dexter" that we're saving in the instance
//rs_copy[Dexter.STOP_TIME] = Date.now() //in milliseconds
///Socket.on_receive(rs_copy, sim_inst.robot.name)
// sim_inst.ack_reply(sim_inst.now_processing_instruction)
//}
sim_inst.completed_instructions.push(sim_inst.now_processing_instruction)
sim_inst.now_processing_instruction = null //Done with cur ins,
sim_inst.ready_to_start_new_instruction = true
Expand Down Expand Up @@ -301,15 +293,14 @@ DexterSim = class DexterSim{
let dur = 10 // in ms
this.now_processing_instruction = this.instruction_queue.shift() //pop off next inst from front of the list
let instruction_array = this.now_processing_instruction
//let robot_status = this.robot_status_in_arcseconds
let oplet = instruction_array[Dexter.INSTRUCTION_TYPE]
//robot_status[Dexter.JOB_ID] = instruction_array[Instruction.JOB_ID]
//robot_status[Dexter.INSTRUCTION_ID] = instruction_array[Instruction.INSTRUCTION_ID]
//robot_status[Dexter.START_TIME] = Date.now() //in ms
//robot_status[Dexter.INSTRUCTION_TYPE] = instruction_array[Instruction.INSTRUCTION_TYPE] //leave this as a 1 char string for now. helpful for debugging
//robot_status[Dexter.ERROR_CODE] = 0

let ins_args = Instruction.args(instruction_array) //in arcseconds
let ins_args = Instruction.args(instruction_array) //in dexter_units
switch (oplet){
case "a": //move_all_joints
//this.robot.angles = ins_args //only set this in move_all_joints and friends
Expand Down Expand Up @@ -422,9 +413,10 @@ DexterSim = class DexterSim{
}

//when we're running the simulator on Dexter
static render_once_node(rs_inst, job_name, robot_name, force_render=true){ //inputs in arc_seconds
static render_once_node(dexter_sim_instance, job_name, robot_name, force_render=true){ //inputs in arc_seconds
//note that SimUtils.render_once has force_render=false, but
//due to other changes, its best if render_once_node default to true
rs_inst = dexter_sim_instance.robot.rs
if (force_render){
//let rs_inst = new RobotStatus({robot_statusa: robot_status})
let j1 = rs_inst.measured_angle(1) //joint_number)robot_status[Dexter.J1_MEASURED_ANGLE]
Expand All @@ -443,54 +435,35 @@ DexterSim = class DexterSim{
}

// also called by process_next_instruction_T()
process_next_instruction_a(ins_args){ //ins_args in arcseconds
/*let robot_status = this.robot_status_in_arcseconds
let idxs
if(this.status_mode === 0) {
idxs = [Dexter.J1_MEASURED_ANGLE, Dexter.J2_MEASURED_ANGLE, Dexter.J3_MEASURED_ANGLE,
Dexter.J4_MEASURED_ANGLE, Dexter.J5_MEASURED_ANGLE, Dexter.J6_MEASURED_ANGLE, Dexter.J7_MEASURED_ANGLE]
}
else if (this.status_mode === 1) {
idxs =[10, 11, 12, 13, 14, 15, 16]
}
//idxs is 7 long. But we don't want to get more than ins_args because those
//extra args aren't actually passed and it will screw up Kin.predict_move_dur to
//pass in 2 arrays of different lengths.
idxs = idxs.slice(0, ins_args.length) //
let orig_angles = [] //in arcseconds
for(let i of idxs) { orig_angles.push(robot_status[i]) }
//set the angles in this robot's robot_status
for(let i = 0; i < ins_args.length; i++){
let angle = ins_args[i] //in arcseconds
if (!isNaN(angle)){
robot_status[idxs[i]] = angle
}
}*/
process_next_instruction_a(angles_dexter_units){
//predict needs its angles in degrees but ins_args are in arcseconds
const orig_angles_in_deg = this.measured_angles_arcseconds.map(function(ang) { return ang / 3600 })
const ins_args_in_deg = ins_args.map(function(ang) { return ang / 3600 })
const orig_angles_in_deg = Socket.dexter_units_to_degrees_array(this.measured_angles_dexter_units) //Socket.dexter_units_to_degrees(this.measured_angles_dexter_units) //this.measured_angles_dexter_units.map(function(ang) { return ang / 3600 })
const angles_in_deg = Socket.dexter_units_to_degrees_array(angles_dexter_units) //ns_args.map(function(ang) { return ang / 3600 })
//ins_args_in_deg[5] = Socket.dexter_units_to_degrees(ins_args[5], 6) //joint 6
//ins_args_in_deg[6] = Socket.dexter_units_to_degrees(ins_args[6], 7) //joint 7

//predict_move_dur takes degrees in and returns seconds
let dur_in_seconds = Math.abs(Kin.predict_move_dur(orig_angles_in_deg, ins_args_in_deg, this.robot))
let dur_in_seconds = Math.abs(Kin.predict_move_dur(orig_angles_in_deg, angles_in_deg, this.robot))
let dur_in_milliseconds = dur_in_seconds * 1000
return dur_in_milliseconds
}

process_next_instruction_T(ins_args){ //ins_args xyz in microns
let xyz_in_microns = [ins_args[0], ins_args[1], ins_args[2]]
let J5_direction = [ins_args[3], ins_args[4], ins_args[5]]
let config = [ins_args[6], ins_args[7], ins_args[8]]
let j6_angle = ins_args[11]
let j7_angle = ins_args[12]
let pose = undefined //its not in the ins_args, and defaults just fine
let J5_direction = [ins_args[3], ins_args[4], ins_args[5]]
let config = [ins_args[6], ins_args[7], ins_args[8]]
let j6_angle = ins_args[11]
let j7_angle = ins_args[12]
let pose = undefined //its not in the ins_args, and defaults just fine

let xyz_in_meters = [xyz_in_microns[0] / 1000000,
xyz_in_microns[1] / 1000000,
xyz_in_microns[2] / 1000000]
let angles_in_degrees = Kin.xyz_to_J_angles(xyz_in_meters, J5_direction, config, pose)
let angles_in_arcseconds = angles_in_degrees.map(function(deg) { return deg * 3600})
angles_in_arcseconds.push(j6_angle)
angles_in_arcseconds.push(j7_angle)
return this.process_next_instruction_a(angles_in_arcseconds)
let angles_in_dexter_units = Socket.degrees_to_dexter_units(angles_in_degrees)
angles_in_dexter_units.push(j6_angle)
angles_in_dexter_units.push(j7_angle)
return this.process_next_instruction_a(angles_in_dexter_units)
}

process_next_instruction_r(instruction_array) {
Expand Down
4 changes: 2 additions & 2 deletions core/index.js
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
global.dde_version = "3.7.4"
global.dde_release_date = "Feb 5, 2021"
global.dde_version = "3.7.5"
global.dde_release_date = "Mar 2, 2021"

console.log("dde_version: " + global.dde_version + " dde_release_date: " + global.dde_release_date +
"\nRead electron_dde/core/job_engine_doc.txt for how to use the Job Engine.\n")
Expand Down
78 changes: 78 additions & 0 deletions core/job.js
Original file line number Diff line number Diff line change
Expand Up @@ -2856,6 +2856,84 @@ Job.is_plausible_when_stopped_value = function(val){
)
}

//retruns an array of arrays, with the outer array having 3 inner arrays,
//one for x, y and z.
Job.prototype.three_d_points_for_plotting = function(which="auto"){ //can also be "orig_do_list" or "sent"
if((which === "auto") && this.sent_instructions) {
which = "sent"
}
else { which = "orig_do_list" }
let xarr=[], yarr=[], zarr=[]
if(which === "orig_do_list") {
let do_list = this.orig_args.do_list
for(let instr of do_list){
let xyz = null
if((instr instanceof Instruction.Dexter) && instr.array_of_angles) {
let angs = instr.array_of_angles
if(angs.length < 5) { //todo imperfect!
angs = angs.slice()
for(let i = 0; i < 5; i++){
if(i === angs.length) {
angs.push(0) //todo hack to get around not having 5 angles because Kin.J_angles_to_xyz will error if we don't
//but really less than 5 should mean "keep that angle in its same position
//see src for move_all_joints
}
}
}
xyz = Kin.J_angles_to_xyz(angs)[0]
}
else if((instr instanceof Instruction.Dexter) && instr.xyz) {
xyz = instr.xyz
}
else if(Instruction.is_oplet_array(instr, "a")){
let angs = Instruction.extract_args(instr)
if(angs.length < 5) { //todo imperfect!
angs = angs.slice()
for(let i = 0; i < 5; i++){
if(i === angs.length) {
angs.push(0) //todo hack to get around not having 5 angles because Kin.J_angles_to_xyz will error if we don't
//but really less than 5 should mean "keep that angle in its same position
//see src for move_all_joints
}
}
}
xyz = Kin.J_angles_to_xyz(angs)[0]
}
if(xyz){
xarr.push(xyz[0])
yarr.push(xyz[1])
zarr.push(xyz[2])
}
}
} // end which === "orig_do_list"
else { //we have which = "sent"
let instrs = this.sent_instructions
for(let instr of instrs){
let xyz = null
if(Instruction.is_oplet_array(instr, "a")){
let angs = Instruction.extract_args(instr)
if(angs.length < 5) { //todo imperfect!
angs = angs.slice()
for(let i = 0; i < 5; i++){
if(i === angs.length) {
angs.push(0) //todo hack to get around not having 5 angles because Kin.J_angles_to_xyz will error if we don't
//but really less than 5 should mean "keep that angle in its same position
//see src for move_all_joints
}
}
}
xyz = Kin.J_angles_to_xyz(angs)[0]
}
if(xyz){
xarr.push(xyz[0])
yarr.push(xyz[1])
zarr.push(xyz[2])
}
}
} //end which == "sent"
return [xarr, yarr, zarr]
}

Job.prototype.to_source_code = function(args={}){
if(!args.indent) { args.indent = "" }
let props_indent = args.indent + " "
Expand Down
20 changes: 14 additions & 6 deletions core/main_eval.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,15 +22,17 @@ def main():
is_py_evalable = False
else:
is_py_evalable = True
if is_py_evalable :
if is_py_evalable:
try:
result = eval(src)
except Exception as err:
result = eval(src)
if str(type(result)) == "<class 'numpy.ndarray'>":
result = result.tolist()
except Exception as err :
is_error = True
result = traceback.format_exc()
#result = result.replace(", line ", ", <br/>line ")
else: #the normal, working, return a value, eval case
is_error = False
is_error = False
else:
try:
exec(src)
Expand All @@ -40,9 +42,15 @@ def main():
#result = result.replace(", line ", ", <br/>line ")
else:
is_error = False
result = None
result = None
json_obj = {"from": "Py.eval", "is_error": is_error, "source": src, "callback_id": callback_id, "result": result}
print(json.dumps(json_obj), sep="", flush=True)
json_str = None
try:
json_str = json.dumps(json_obj)
except Exception as err:
json_obj = {"from": "Py.eval", "is_error": True, "source": src, "callback_id": callback_id, "result": "Could not make JSON string of the result."}
json_str = json.dumps(json_obj)
print(json_str, sep="", flush=True)

print("Python: main_eval.py file loaded.", "\n", flush=True)
main()
Expand Down
14 changes: 7 additions & 7 deletions core/py.js
Original file line number Diff line number Diff line change
Expand Up @@ -61,9 +61,11 @@ class Py{
}

static init_class(){ //does not init the process, just the Py class
if (operating_system === "win") { Py.python_executable_path = "python" }
else if (operating_system === "mac") { Py.python_executable_path = "python3" } //python gets you python2.7
else if (operating_system === "linux") { Py.python_executable_path = "python" }
if(!Py.python_executable_path){
if (operating_system === "win") { Py.python_executable_path = "python" }
else if (operating_system === "mac") { Py.python_executable_path = "python3" } //python gets you python2.7
else /*(operating_system === "linux")*/ { Py.python_executable_path = "python3" }
}
if(!Py.main_eval_py_path) { Py.main_eval_py_path = dde_apps_folder + "/main_eval.py" }
//Py.main_eval_py_path = __dirname + "/main_eval.py" //note that __dirname, when inside the job engine core folder, ends in "/core" so don't stick that on the end.
out('Py.python_executable_path set to: <code>' + Py.python_executable_path + '</code>')
Expand All @@ -77,9 +79,7 @@ class Py{
//document
static init(){
this.kill()
if(!Py.python_executable_path) {
Py.init_class()
}
Py.init_class()
this.process = spawn(this.python_executable_path, [Py.main_eval_py_path]) //[dde_apps_folder + "/main_eval.py"]);
out("New Python process created.")
this.process.stdout.on('data', (data) => {
Expand Down Expand Up @@ -190,7 +190,7 @@ class Py{
//path should end in .py
static load_file(path, as_name = null, callback=Py.default_callback){
path = make_full_path(path, false) //don't adjust to OS, keep as slashes.
path = replace_substrings(path, "\\", "/")
path = replace_substrings(path, "\\", "/", false)
this.eval("sys.path",
function(json_obj){
let folder_array = json_obj.result
Expand Down
26 changes: 21 additions & 5 deletions core/robot.js
Original file line number Diff line number Diff line change
Expand Up @@ -338,6 +338,22 @@ var Brain = class Brain extends Robot { /*no associated hardware */
out(reason, "red")
throw new Error("send called on Robot.Brain, which has no physical robot.")
}

static eval_python(python_source, user_data_variable="python_value"){
return [
function() {
let the_job = this
the_job.user_data[user_data_variable + "_python_source"] = python_source
Py.eval(python_source,
function(json_obj){
the_job.user_data[user_data_variable] = json_obj.result
})
},
Control.wait_until(function() {
//out("this.user_data." + user_data_variable = ": " + this.user_data.[user_data_variable])
return this.user_data[user_data_variable] !== undefined})
]
}
}

Brain.all_names = []
Expand Down Expand Up @@ -2454,8 +2470,8 @@ Dexter.prototype.set_link_lengths_using_dde_db = function(job_to_start){
this.J3_angle_max = Dexter.J3_ANGLE_MAX
this.J4_angle_max = Dexter.J4_ANGLE_MAX
this.J5_angle_max = Dexter.J5_ANGLE_MAX
this.J6_angle_min = Dexter.J6_ANGLE_MAX
this.J7_angle_min = Dexter.J7_ANGLE_MAX
this.J6_angle_max = Dexter.J6_ANGLE_MAX
this.J7_angle_max = Dexter.J7_ANGLE_MAX
}
this.link_lengths_set_from_dde_computer = true
if(job_to_start) {
Expand Down Expand Up @@ -2528,9 +2544,9 @@ Dexter.J6_ANGLE_MAX = 296
Dexter.J7_ANGLE_MIN = 0
Dexter.J7_ANGLE_MAX = 296

Dexter.MAX_SPEED = 30 //degrees per second. NOT the max speed tha robot,
//but rather for a givien instruction's envelope of speed,
//its the max speed that will be attined by that instrution.
Dexter.MAX_SPEED = 30 //degrees per second. NOT the max speed of the robot,
//but rather for a given instruction's envelope of speed,
//its the max speed that will be attined by that instruction.
//The JOINT that this is the max speed for is
//the joint that changes the most in a given call to move_all_joints.
Dexter.START_SPEED = 0.5 //degrees per second
Expand Down
Loading

0 comments on commit ae345d8

Please sign in to comment.