| """Variable topology 1--15 DOF robotics rigid-body inverse dynamics simulator. |
| |
| Independent, reduced-order ideal revolute joint rigid-body RNEA-like Newton-Euler |
| backward recursion. NOT MuJoCo or Pinocchio; excludes contact, gearing backlash, |
| flexible joints, hard stops and servo controller transients. |
| |
| Units: m, kg, seconds, rad, N, Nm, W and Wh. Topology supplied as rooted tree. |
| Joint axes are expressed in parent frame; inertia in child link frame. |
| """ |
| from __future__ import annotations |
| import math |
| import numpy as np |
| from numba import njit |
|
|
| MAX_JOINTS=15 |
|
|
| @njit(cache=True) |
| def _cross(a,b): |
| return np.array([a[1]*b[2]-a[2]*b[1],a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0]]) |
|
|
| @njit(cache=True) |
| def _rot(axis,theta): |
| c=math.cos(theta);s=math.sin(theta) |
| x,y,z=axis[0],axis[1],axis[2] |
| K=np.array([[0.,-z,y],[z,0.,-x],[-y,x,0.]]) |
| I=np.eye(3) |
| return I+s*K+(1.-c)*(K@K) |
|
|
| @njit(cache=True) |
| def rnea_tree(parents, offsets, axes, coms, mass, inertia, q, dq, ddq): |
| """Return inverse-dynamics joint torques for rooted revolute tree. |
| All joint positions are at their preceding-link pivot, offset in parent coords. |
| """ |
| n=len(q) |
| r=np.zeros((n,3,3)); pivot=np.zeros((n,3));axis_w=np.zeros((n,3)) |
| omega=np.zeros((n,3));alpha=np.zeros((n,3));acc=np.zeros((n,3)) |
| force=np.zeros((n,3)); moment=np.zeros((n,3)); tau=np.zeros(n) |
| gravity=np.array([0.,0.,-9.81]) |
| for i in range(n): |
| p=parents[i] |
| if p>=0: |
| R0=r[p]; wp=omega[p];ap=alpha[p]; a0=acc[p];pivot0=pivot[p] |
| else: |
| R0=np.eye(3);wp=np.zeros(3);ap=np.zeros(3);a0=np.zeros(3);pivot0=np.zeros(3) |
| arm=R0@offsets[i] |
| pivot[i]=pivot0+arm |
| axis_w[i]=R0@axes[i] |
| r[i]=R0@_rot(axes[i],q[i]) |
| omega[i]=wp+axis_w[i]*dq[i] |
| alpha[i]=ap+axis_w[i]*ddq[i]+_cross(wp,axis_w[i]*dq[i]) |
| acc[i]=a0+_cross(ap,arm)+_cross(wp,_cross(wp,arm)) |
| rc=r[i]@coms[i] |
| ic=r[i]@np.diag(inertia[i])@r[i].T |
| ci=acc[i]+_cross(alpha[i],rc)+_cross(omega[i],_cross(omega[i],rc)) |
| force[i]=mass[i]*(ci-gravity) |
| moment[i]=ic@alpha[i]+_cross(omega[i],ic@omega[i])+_cross(rc,force[i]) |
| for i in range(n-1,-1,-1): |
| tau[i]=np.dot(axis_w[i],moment[i]) |
| p=parents[i] |
| if p>=0: |
| force[p]+=force[i] |
| moment[p]+=moment[i]+_cross(pivot[i]-pivot[p],force[i]) |
| return tau |
|
|
|
|
| def _unit(v): |
| a=np.asarray(v,float);return a/np.linalg.norm(a) |
|
|
|
|
| def robot_configuration(n, topology, rng, geometry_shift=False): |
| """Build physically plausible but non-OEM synthetic kinematic tree.""" |
| if not 1<=n<=15:raise ValueError('n must lie in 1..15') |
| if topology=='serial': lengths=[n];torso=False |
| elif topology=='dual': |
| if n<2:raise ValueError('dual requires at least two joints') |
| na=min(7,max(1,n//2));nb=n-na |
| if nb>7:raise ValueError('dual arm supports no more than 14 joints') |
| lengths=[na,nb];torso=False |
| elif topology=='torso_dual': |
| if n<3:raise ValueError('torso_dual needs torso and two arms') |
| na=min(7,max(1,(n-1)//2));nb=n-1-na |
| if nb>7:raise ValueError('torso arms <=7 each') |
| lengths=[na,nb];torso=True |
| else:raise ValueError(topology) |
| pa=[];offs=[];axes=[];com=[];ms=[];ins=[];ids=[];armids=[] |
| if torso: |
| pa.append(-1);offs.append([0,0,0.45]);axes.append([0,0,1]);com.append([0.03,0,0]);ms.append(float(rng.uniform(20,45)));ins.append([1.6,1.6,2.2]);ids.append('torso');armids.append(2) |
| for aid,L in enumerate(lengths): |
| prev=0 if torso else -1 |
| for j in range(L): |
| link=float(rng.uniform(.20,.50)*(1.20 if geometry_shift else 1)) |
| mass=float(rng.uniform(1.5,11.)*(1.25 if geometry_shift else 1)) |
| if j==0 and len(lengths)==2: |
| off=[0,(-0.32 if aid==0 else +0.32),0.12 if torso else 0.50] |
| elif j==0:off=[0,0,0.48] |
| else:off=[last_length,0,0] |
| axislist=[[0,0,1],[0,1,0],[1,0,0],[0,1,0],[1,0,0],[0,1,0],[0,0,1]] |
| axis=axislist[j%7] |
| pa.append(prev);offs.append(off);axes.append(axis) |
| com.append([link*.50,0,0]);ms.append(mass) |
| I=mass*link*link/12 |
| ins.append([max(.002,I*.12),max(.002,I),max(.002,I)]) |
| idx=len(pa)-1;ids.append(f'arm{aid}_joint{j+1}');armids.append(aid) |
| prev=idx;last_length=link |
| if len(pa)!=n:raise AssertionError('wrong joint count') |
| return {'n':n,'topology':topology,'parents':pa,'offsets':offs,'axes':axes,'coms':com,'mass':ms, |
| 'inertia':ins,'joint_names':ids,'arm_id':armids,'payload_kg':0., |
| 'qmax':float(rng.uniform(.8,1.8)),'amax':float(rng.uniform(2.,4.5)), |
| 'motor_nominal_eff':float(rng.uniform(.82,.93)), |
| 'electronic_power_W':float(rng.uniform(90.,240.)), |
| 'regen_capability':float(rng.uniform(.30,.75)), |
| 'private_motor_efficiency':float(rng.uniform(.79,1.08)), |
| 'private_driver_loss':float(rng.uniform(.75,1.28)), |
| 'private_friction_scale':float(rng.uniform(.78,1.27)), |
| 'private_settle_s':float(rng.uniform(.015,.145)), |
| 'private_regen_scale':float(rng.uniform(.75,1.12))} |
|
|
|
|
| def quintic_trajectory(q0,q1,duration,steps=22): |
| s=np.linspace(0.,1.,steps) |
| blend=10*s**3-15*s**4+6*s**5 |
| db=(30*s**2-60*s**3+30*s**4)/duration |
| ddb=(60*s-180*s**2+120*s**3)/(duration**2) |
| move=q1-q0 |
| return q0[None,:]+blend[:,None]*move[None,:], db[:,None]*move[None,:],ddb[:,None]*move[None,:] |
|
|
|
|
| def simulate(cfg, motion, steps=22, detailed=False): |
| """Compute forward specified trajectory inverse dynamics + electrical model. |
| |
| Motion is an independent plan: q_start/q_end, speed_factor, dwell_s and payload. |
| The private efficiency parameters are intentionally unavailable to training code. |
| """ |
| n=cfg['n'];q0=np.asarray(motion['q_start'],float);q1=np.asarray(motion['q_end'],float) |
| disp=np.abs(q1-q0);speed=float(motion['speed_factor']); |
| maxv=cfg['qmax']*speed;maxa=cfg['amax']*speed |
| move_t=max(.18, 1.875*disp.max()/maxv,math.sqrt(5.78*disp.max()/maxa)) |
| dwell=float(motion['dwell_s']);q,dq,ddq=quintic_trajectory(q0,q1,move_t,steps) |
| pa=np.array(cfg['parents'],np.int64);offset=np.array(cfg['offsets'],float) |
| axis=np.array(cfg['axes'],float);com=np.array(cfg['coms'],float);masses=np.array(cfg['mass'],float) |
| inertia=np.array(cfg['inertia'],float) |
| payload=float(motion['payload_kg']) |
| |
| ends=[i for i in range(n) if i not in pa] |
| for idx in ends: |
| masses[idx]+=payload/len(ends) |
| inertia[idx]+=payload*.012/len(ends) |
| torques=np.stack([rnea_tree(pa,offset,axis,com,masses,inertia,q[i],dq[i],ddq[i]) for i in range(steps)]) |
| fric=(.12+.015*masses[None,:])*np.tanh(dq*18.)+.08*dq |
| tau_motor=torques+fric*cfg['private_friction_scale'] |
| mechanical=tau_motor*dq |
| nominal_tau=torques+fric |
| positive=np.maximum(mechanical,0.);negative=np.minimum(mechanical,0.) |
| |
| efficiency=float(np.clip(cfg['motor_nominal_eff']*cfg['private_motor_efficiency'],.60,.98)) |
| copper=.020*tau_motor**2*cfg['private_driver_loss'] |
| drive_idle=(2.0+.35*masses[None,:])*cfg['private_driver_loss'] |
| regen=cfg['regen_capability']*cfg['private_regen_scale'] |
| electric=positive/efficiency+negative*regen+copper+drive_idle |
| |
| bus=np.maximum(electric.sum(axis=1),0.) |
| control=cfg['electronic_power_W']+16.*n |
| power=bus+control |
| energy=float(np.trapezoid(power,dx=move_t/(steps-1))/3600.) |
| settle=cfg['private_settle_s']*(1.+.08*n/15.) |
| settle_e=settle*(control+np.sum(drive_idle))/3600. |
| dwell_e=dwell*(control+np.sum(drive_idle))/3600. |
| true_energy=energy+settle_e+dwell_e |
| true_time=move_t+settle+dwell |
| |
| nominal_electric=np.maximum(nominal_tau*dq,0.)/cfg['motor_nominal_eff']+np.minimum(nominal_tau*dq,0.)*cfg['regen_capability']+.020*nominal_tau**2+2.+.35*masses[None,:] |
| nominal_energy=(np.trapezoid(np.maximum(nominal_electric.sum(axis=1),0.)+control,dx=move_t/(steps-1))+dwell*(control+np.sum(2.+.35*masses)))/3600. |
| mot_nom=nominal_tau*dq |
| nom_pos=float(np.trapezoid(np.maximum(mot_nom,0).sum(axis=1),dx=move_t/(steps-1))) |
| nom_regen=float(np.trapezoid(np.maximum(-mot_nom,0).sum(axis=1),dx=move_t/(steps-1))) |
| nom_copper=float(np.trapezoid((.020*nominal_tau**2).sum(axis=1),dx=move_t/(steps-1))) |
| nom_idle=float((move_t+dwell)*np.sum(2.+.35*masses)) |
| nom_aux=float((move_t+dwell)*control) |
| out={'time_s':true_time,'energy_Wh':true_energy,'nominal_time_s':move_t+dwell,'nominal_energy_Wh':float(nominal_energy), |
| 'mechanical_positive_J':float(np.trapezoid(positive.sum(axis=1),dx=move_t/(steps-1))), |
| 'mechanical_regen_J':float(np.trapezoid(-negative.sum(axis=1),dx=move_t/(steps-1))), |
| 'torque_rms_Nm':np.sqrt((torques**2).mean(axis=0)).tolist(), |
| 'torque_max_Nm':np.max(np.abs(torques),axis=0).tolist(), |
| 'velocity_rms_rad_s':np.sqrt((dq**2).mean(axis=0)).tolist(), |
| 'mean_abs_velocity_rad_s':np.mean(np.abs(dq),axis=0).tolist(), |
| 'torque_work_J':np.trapezoid(np.abs(nominal_tau*dq),dx=move_t/(steps-1),axis=0).tolist(), |
| 'torque_rms_sq_Nm2':np.mean(nominal_tau**2,axis=0).tolist(), |
| 'material_removed_g':0., |
| 'nominal_positive_J':nom_pos,'nominal_regen_J':nom_regen,'nominal_copper_J':nom_copper, |
| 'nominal_idle_J':nom_idle,'nominal_aux_J':nom_aux} |
| if detailed: |
| out['trace']={'time_s':np.linspace(0,move_t,steps).tolist(),'torque_Nm':torques.tolist(), |
| 'joint_position_rad':q.tolist(),'joint_velocity_rad_s':dq.tolist(), 'bus_power_W':power.tolist()} |
| return out |
|
|
|
|
| def sensor_probes(cfg,rng): |
| """Six *past* power measurements (Watts per motor, normalized against size). |
| Standstill, low/medium/high joint speed, loaded, braking/regen. |
| These measurements exclude target program observations and private factors. |
| """ |
| n=cfg['n'];eff=cfg['motor_nominal_eff']*cfg['private_motor_efficiency'] |
| friction=cfg['private_friction_scale'];loss=cfg['private_driver_loss'] |
| base=(cfg['electronic_power_W']+16*n)/n+(2+.35*np.mean(cfg['mass']))*loss |
| probes=[base,base+7*friction+5*loss,base+18/eff+13*loss,base+35/eff+24*loss, |
| base+52/eff+46*loss,base+19/eff+16*loss-8*cfg['regen_capability']*cfg['private_regen_scale']] |
| noise=rng.normal(0,.006,6) |
| return np.maximum(np.array(probes)*(1+noise),.001).tolist() |
|
|
