if (sim_call_type==sim_childscriptcall_initialization) then steer_handle= simGetObjectHandle('steer_joint') motor_handle= simGetObjectHandle('motor_joint') fl_brake_handle= simGetObjectHandle('fl_brake_joint') fr_brake_handle= simGetObjectHandle('fr_brake_joint') bl_brake_handle= simGetObjectHandle('bl_brake_joint') br_brake_handle= simGetObjectHandle('br_brake_joint') --wheel radius: 0.09 --wheel base: 0.6 --wheel track: 0.35 --maximum steering rate: 70 deg/sec --the maximum steer angle 30 degree max_steer_angle=0.5235987 --the maximum torque of the motor motor_torque=60 dVel=1 dSteer=0.1 --input steer steer_angle=0 --input velocity motor_velocity=dVel*10 --input brake brake_force=0 end if (sim_call_type==sim_childscriptcall_cleanup) then end if (sim_call_type==sim_childscriptcall_actuation) then --current steer pos steer_pos=simGetJointPosition(steer_handle); --current angular velocity of back left wheel bl_wheel_velocity=simGetObjectFloatParameter(bl_brake_handle,2012) --current angular velocity of back right wheel br_wheel_velocity=simGetObjectFloatParameter(br_brake_handle,2012) --average angular velocity of the back wheels rear_wheel_velocity=(bl_wheel_velocity+br_wheel_velocity)/2 --linear velocity linear_velocity=rear_wheel_velocity*0.09 -- Read the keyboard messages (make sure the focus is on the main window, scene view): message,auxiliaryData=simGetSimulatorMessage() while message~=-1 do if (message==sim_message_keypress) then Msg="Tecla: " Msg=Msg..auxiliaryData[1] simAddStatusbarMessage(Msg) if (auxiliaryData[1]==2007) then -- up key if (motor_velocity-dVel*1.99) then motor_velocity=motor_velocity-dVel else -- brake_force=100 end end if (auxiliaryData[1]==2009) then -- left key if (steer_angle-dSteer*4.99) then steer_angle=steer_angle-dSteer end end if (auxiliaryData[1]==115) then -- s key motor_velocity=0.0 end end message,auxiliaryData=simGetSimulatorMessage() end if (math.abs(motor_velocity) max_steer_angle) then steer_angle=max_steer_angle end if (steer_angle< -max_steer_angle) then steer_angle= -max_steer_angle end simSetJointTargetPosition(steer_handle, steer_angle) --brake and motor can not be applied at the same time if(brake_force>0) then simSetJointForce(motor_handle, 0) else simSetJointForce(motor_handle, motor_torque) simSetJointTargetVelocity(motor_handle, motor_velocity) end simSetJointForce(fr_brake_handle, brake_force) simSetJointForce(fl_brake_handle, brake_force) simSetJointForce(bl_brake_handle, brake_force) simSetJointForce(br_brake_handle, brake_force) end