def publishState(self, subsampling=100): if hasattr(self, "ros_publish_state"): return from dynamic_graph.ros import RosPublish self.ros_publish_state = RosPublish("ros_publish_state") self.ros_publish_state.add("vector", "state", "/sot_hpp/state") plug(self.sotrobot.device.state, self.ros_publish_state.state) self.sotrobot.device.after.addDownsampledSignal( "ros_publish_state.trigger", 100)
def test_balance_ctrl_talos_gazebo(robot, use_real_vel=True, use_real_base_state=False, startSoT=True): # BUILD THE STANDARD GRAPH conf = get_sim_conf() robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf) # force current measurements to zero robot.ctrl_manager.i_measured.value = NJ * (0.0, ) #robot.current_ctrl.i_measured.value = NJ*(0.0,); robot.filters.current_filter.x.value = NJ * (0.0, ) # BYPASS TORQUE CONTROLLER plug(robot.inv_dyn.tau_des, robot.ctrl_manager.ctrl_torque) # CREATE SIGNALS WITH ROBOT STATE WITH CORRECT SIZE (36) robot.q = Selec_of_vector("q") plug(robot.device.robotState, robot.q.sin) robot.q.selec(0, NJ + 6) plug(robot.q.sout, robot.pos_ctrl.base6d_encoders) plug(robot.q.sout, robot.traj_gen.base6d_encoders) #plug(robot.q.sout, robot.estimator_ft.base6d_encoders); robot.ros = RosPublish('rosPublish') robot.device.after.addDownsampledSignal('rosPublish.trigger', 1) # BYPASS JOINT VELOCITY ESTIMATOR if (use_real_vel): robot.dq = Selec_of_vector("dq") #plug(robot.device.robotVelocity, robot.dq.sin); # to check robotVelocity empty plug(robot.device.velocity, robot.dq.sin) robot.dq.selec(6, NJ + 6) plug(robot.dq.sout, robot.pos_ctrl.jointsVelocities) # generate seg fault plug(robot.dq.sout, robot.base_estimator.joint_velocities) plug(robot.device.gyrometer, robot.base_estimator.gyroscope) # BYPASS BASE ESTIMATOR robot.v = Selec_of_vector("v") #plug(robot.device.robotVelocity, robot.dq.sin); # to check robotVelocity empty plug(robot.device.velocity, robot.dq.sin) robot.v.selec(0, NJ + 6) if (use_real_base_state): plug(robot.q.sout, robot.inv_dyn.q) plug(robot.v.sout, robot.inv_dyn.v) if (startSoT): start_sot() # RESET FORCE/TORQUE SENSOR OFFSET # sleep(10*robot.timeStep); #robot.estimator_ft.setFTsensorOffsets(24*(0.0,)); return robot
def publishState(self, subsampling=40): if hasattr(self, "ros_publish_state"): return from dynamic_graph.ros import RosPublish self.ros_publish_state = RosPublish("ros_publish_state") self.ros_publish_state.add("vector", "state", "/agimus/sot/state") self.ros_publish_state.add("vector", "reference_state", "/agimus/sot/reference_state") plug(self.sotrobot.device.state, self.ros_publish_state.state) plug(self.rosSubscribe.posture, self.ros_publish_state.reference_state) self.sotrobot.device.after.addDownsampledSignal( "ros_publish_state.trigger", subsampling)
def __init__(self, name, robot): self.name = name # Setup entity that triggers the event. self.switch = SwitchBoolean(name + "_switch") self.event = Event(name + "_event") plug(self.switch.sout, self.event.condition) robot.device.after.addSignal(name + "_event.check") self.ros_publish = RosPublish(name + '_ros_publish') # self.ros_publish.add ('boolean', name, '/agimus/sot/event/' + name) # self.ros_publish.signal(name).value = int(True) self.ros_publish.add('int', name, '/agimus/sot/event/' + name) self.event.addSignal(name + "_ros_publish.trigger") self.switch_string = {}
def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False): # BUILD THE STANDARD GRAPH conf = get_sim_conf(); robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf); # force current measurements to zero robot.ctrl_manager.currents.value = NJ*(0.0,); # BYPASS TORQUE CONTROLLER plug(robot.inv_dyn.tau_des, robot.ctrl_manager.ctrl_torque); # CREATE SIGNALS WITH ROBOT STATE WITH CORRECT SIZE (36) robot.q = Selec_of_vector("q"); plug(robot.device.robotState, robot.q.sin); robot.q.selec(0, NJ+6); plug(robot.q.sout, robot.pos_ctrl.base6d_encoders); plug(robot.q.sout, robot.traj_gen.base6d_encoders); plug(robot.q.sout, robot.estimator_ft.base6d_encoders); plug(robot.q.sout, robot.ctrl_manager.base6d_encoders); plug(robot.q.sout, robot.torque_ctrl.base6d_encoders); robot.ros = RosPublish('rosPublish'); robot.device.after.addDownsampledSignal('rosPublish.trigger',1); # BYPASS JOINT VELOCITY ESTIMATOR if(use_real_vel): robot.dq = Selec_of_vector("dq"); plug(robot.device.robotVelocity, robot.dq.sin); robot.dq.selec(6, NJ+6); plug(robot.dq.sout, robot.pos_ctrl.jointsVelocities); plug(robot.dq.sout, robot.base_estimator.joint_velocities); # BYPASS BASE ESTIMATOR robot.v = Selec_of_vector("v"); plug(robot.device.robotVelocity, robot.v.sin); robot.v.selec(0, NJ+6); if(use_real_base_state): plug(robot.q.sout, robot.inv_dyn.q); plug(robot.v.sout, robot.inv_dyn.v); # start_sot(); # # # RESET FORCE/TORQUE SENSOR OFFSET # sleep(10*robot.timeStep); # robot.estimator_ft.setFTsensorOffsets(24*(0.0,)); return robot;
def init_appli(robot): taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) handMgrip = eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') # --- STATIC COM (if not walking) taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) taskCom.featureDes.errorIN.value = robot.dynamic.com.value taskCom.task.controlGain.value = 10 # --- CONTACTS contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) contactLF.feature.frame('desired') contactLF.gain.setConstant(10) contactLF.keep() locals()['contactLF'] = contactLF contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) contactRF.feature.frame('desired') contactRF.gain.setConstant(10) contactRF.keep() locals()['contactRF'] = contactRF from dynamic_graph import plug from dynamic_graph.sot.core.sot import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(sot.control, robot.device.control) from dynamic_graph.ros import RosPublish ros_publish_state = RosPublish("ros_publish_state") ros_publish_state.add("vector", "state", "/sot_control/state") from dynamic_graph import plug plug(robot.device.state, ros_publish_state.state) robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100) sot.push(contactRF.task.name) sot.push(contactLF.task.name) sot.push(taskCom.task.name) robot.device.control.recompute(0)
def setupEvents(self): from dynamic_graph_hpp.sot import Event, CompareDouble from dynamic_graph.sot.core.operator import Norm_of_vector from dynamic_graph.ros import RosPublish self.norm = Norm_of_vector("control_norm") plug(self.sotrobot.device.control, self.norm.sin) self.norm_comparision = CompareDouble("control_norm_comparison") plug(self.norm.sout, self.norm_comparision.sin1) self.norm_comparision.sin2.value = 1e-2 self.norm_event = Event("control_norm_event") plug(self.norm_comparision.sout, self.norm_event.condition) # self.sotrobot.device.after.addSignal (self.norm_event.check.name) self.sotrobot.device.after.addSignal("control_norm_event.check") self.ros_publish = RosPublish('ros_publish_control_norm') self.ros_publish.add('double', 'event_control_norm', '/sot_hpp/control_norm_changed') plug(self.norm.sout, self.ros_publish.event_control_norm) # plug (self.norm_event.trigger, self.ros_publish.trigger) self.norm_event.addSignal("ros_publish_control_norm.trigger")
# flake8: noqa from dynamic_graph import plug from dynamic_graph.ros import RosPublish robot.ros = RosPublish('rosPublish') robot.ros.add('vector', 'robotState_ros', 'robotState') plug(robot.device.robotState, robot.ros.signal('robotState_ros')) robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['wrist']) handMgrip = eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') projection = HolonomicProjection("projection") projection.setSize(robot.dynamic.getDimension()) projection.setLeftWheel(6) projection.setRightWheel(7) # The wheel separation could be obtained with pinocchio. # See pmb2_description/urdf/base.urdf.xacro projection.setWheelRadius(0.0985) projection.setWheelSeparation(0.4044) plug(robot.dynamic.mobilebase, projection.basePose) sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(projection.projection, sot.proj0) plug(sot.control, robot.device.control) ros_publish_state = RosPublish("ros_publish_state") ros_publish_state.add("vector", "state", "/sot_control/state") plug(robot.device.state, ros_publish_state.state) robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100) robot.device.control.recompute(0)
def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None): if (conf is None): conf = get_default_conf() dt = robot.timeStep # TMP: overwrite halfSitting configuration to use SoT joint order robot.halfSitting = ( # Free flyer 0., 0., 0.648702, 0., 0., 0., # Legs 0., 0., -0.453786, 0.872665, -0.418879, 0., 0., 0., -0.453786, 0.872665, -0.418879, 0., # Chest and head 0., 0., 0., 0., # Arms 0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1, 0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1) robot.device.setControlInputType('noInteg') robot.ctrl_manager = create_ctrl_manager(conf.control_manager, conf.motor_params, dt) robot.traj_gen = create_trajectory_generator(robot.device, dt) robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt) robot.rf_force_traj_gen = create_force_traj_gen( "rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt) robot.lf_force_traj_gen = create_force_traj_gen( "lf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt) robot.traj_sync = create_trajectory_switch() robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf") robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf") robot.rf_traj_gen.init(dt) robot.lf_traj_gen.init(dt) robot.encoders = create_encoders(robot) robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt) (robot.estimator_ft, robot.filters) = create_estimators(robot, conf.force_torque_estimator, conf.motor_params, dt) robot.imu_filter = create_imu_filter(robot, dt) robot.base_estimator = create_base_estimator(robot, dt, conf.base_estimator) connect_synchronous_trajectories(robot.traj_sync, [ robot.com_traj_gen, robot.rf_force_traj_gen, robot.lf_force_traj_gen, robot.rf_traj_gen, robot.lf_traj_gen ]) #robot.rf_traj_gen, robot.lf_traj_gen]) robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt) robot.torque_ctrl = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt) robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl, conf.motor_params, dt) robot.adm_ctrl = create_admittance_ctrl(robot, conf.adm_ctrl, dt) robot.current_ctrl = create_current_controller(robot, conf.current_ctrl, conf.motor_params, dt) connect_ctrl_manager(robot) # create low-pass filter for computing joint velocities robot.encoder_filter = create_chebi2_lp_filter_Wn_03_N_4( 'encoder_filter', dt, conf.motor_params.NJ) plug(robot.encoders.sout, robot.encoder_filter.x) plug(robot.encoder_filter.dx, robot.current_ctrl.dq) plug(robot.encoder_filter.dx, robot.torque_ctrl.jointsVelocities) #plug(robot.encoder_filter.x_filtered, robot.base_estimator.joint_positions); #plug(robot.encoder_filter.dx, robot.base_estimator.joint_velocities); robot.ros = RosPublish('rosPublish') robot.device.after.addDownsampledSignal('rosPublish.trigger', 1) robot.estimator_ft.dgyro.value = (0.0, 0.0, 0.0) robot.estimator_ft.gyro.value = (0.0, 0.0, 0.0) # estimator.accelerometer.value = (0.0, 0.0, 9.81); if (startSoT): print "Gonna start SoT" sleep(1.0) start_sot() if (go_half_sitting): print "Gonna go to half sitting in 1 sec" sleep(1.0) go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0) return robot
def create_ros_topics(robot): from dynamic_graph.ros import RosPublish ros = RosPublish('rosPublish') try: create_topic(ros, robot.device.robotState, 'robotState') create_topic(ros, robot.device.gyrometer, 'gyrometer') create_topic(ros, robot.device.accelerometer, 'accelerometer') create_topic(ros, robot.device.forceRLEG, 'forceRLEG') create_topic(ros, robot.device.forceLLEG, 'forceLLEG') create_topic(ros, robot.device.currents, 'currents') # create_topic(ros, robot.device.forceRARM, 'forceRARM'); # create_topic(ros, robot.device.forceLARM, 'forceLARM'); robot.device.after.addDownsampledSignal('rosPublish.trigger', 1) except: pass try: create_topic(ros, robot.filters.estimator_kin.dx, 'jointsVelocities') # create_topic(ros, robot.estimator.jointsTorquesFromInertiaModel, 'jointsTorquesFromInertiaModel'); # create_topic(ros, robot.estimator.jointsTorquesFromMotorModel, 'jointsTorquesFromMotorModel'); # create_topic(ros, robot.estimator.currentFiltered, 'currentFiltered'); except: pass try: create_topic(ros, robot.torque_ctrl.u, 'i_des_torque_ctrl') except: pass try: create_topic(ros, robot.traj_gen.q, 'q_ref') # create_topic(ros, robot.traj_gen.dq, 'dq_ref'); # create_topic(ros, robot.traj_gen.ddq, 'ddq_ref'); except: pass try: create_topic(ros, robot.ctrl_manager.pwmDes, 'i_des') create_topic(ros, robot.ctrl_manager.pwmDesSafe, 'i_des_safe') # create_topic(ros, robot.ctrl_manager.signOfControlFiltered, 'signOfControlFiltered'); # create_topic(ros, robot.ctrl_manager.signOfControl, 'signOfControl'); except: pass try: create_topic(ros, robot.inv_dyn.tau_des, 'tau_des') except: pass try: create_topic(ros, robot.ff_locator.base6dFromFoot_encoders, 'base6dFromFoot_encoders') except: pass try: create_topic(ros, robot.floatingBase.soutPos, 'floatingBase_pos') except: pass return ros
def create_rospublish(robot, name): from dynamic_graph.ros import RosPublish rospub = RosPublish(name) robot.device.after.addSignal(rospub.name + '.trigger') return rospub
def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None): if(conf is None): conf = get_default_conf(); dt = robot.timeStep; # TMP: overwrite halfSitting configuration to use SoT joint order robot.halfSitting = ( # Free flyer 0., 0., 1.018213, 0., 0. , 0., # legs 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # Chest 0.0 , 0.006761, # arms 0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, -0.005, -0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005, # Head 0.0,0.0); #robot.device.setControlInputType('noInteg'); robot.ctrl_manager = create_ctrl_manager(conf.control_manager, conf.motor_params, dt); robot.traj_gen = create_trajectory_generator(robot.device, dt); robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt); robot.rf_force_traj_gen = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt); robot.lf_force_traj_gen = create_force_traj_gen("lf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt); robot.traj_sync = create_trajectory_switch(); robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf"); robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf"); robot.rf_traj_gen.init(dt); robot.lf_traj_gen.init(dt); robot.encoders = create_encoders(robot); robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt); #(robot.estimator_ft, robot.filters) = create_estimators(robot, conf.force_torque_estimator, conf.motor_params, dt); robot.filters = create_filters(robot, conf.force_torque_estimator, conf.motor_params, dt); robot.imu_filter = create_imu_filter(robot, dt); robot.base_estimator = create_base_estimator(robot, dt, conf.base_estimator); connect_synchronous_trajectories(robot.traj_sync, [robot.com_traj_gen, robot.rf_force_traj_gen, robot.lf_force_traj_gen, robot.rf_traj_gen, robot.lf_traj_gen]) #robot.rf_traj_gen, robot.lf_traj_gen]) robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt); robot.torque_ctrl = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt); robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,conf.motor_params, dt); #robot.current_ctrl = create_current_controller(robot, conf.current_ctrl, conf.motor_params, dt); connect_ctrl_manager(robot); # create low-pass filter for computing joint velocities robot.encoder_filter = create_chebi2_lp_filter_Wn_03_N_4('encoder_filter', dt, conf.motor_params.NJ) plug(robot.encoders.sout, robot.encoder_filter.x) #plug(robot.encoder_filter.dx, robot.current_ctrl.dq); plug(robot.encoder_filter.dx, robot.torque_ctrl.jointsVelocities); #plug(robot.encoder_filter.x_filtered, robot.base_estimator.joint_positions); #plug(robot.encoder_filter.dx, robot.base_estimator.joint_velocities); robot.ros = RosPublish('rosPublish'); robot.device.after.addDownsampledSignal('rosPublish.trigger',1); #robot.estimator_ft.dgyro.value = (0.0, 0.0, 0.0); #robot.estimator_ft.gyro.value = (0.0, 0.0, 0.0); # estimator.accelerometer.value = (0.0, 0.0, 9.81); if(startSoT): print "Gonna start SoT"; sleep(1.0); start_sot(); if(go_half_sitting): print "Gonna go to half sitting in 1 sec"; sleep(1.0); go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0); return robot;