#traj_files = ['trajectories/traj1'+'_x'+str(box_position[0])+'_y'+str(box_position[1])+'_Y'+str(box_yaw)+'_m'+str(reach_method)+'_reach.npy', # 'trajectories/traj1'+'_x'+str(box_position[0])+'_y'+str(box_position[1])+'_Y'+str(box_yaw)+'_m'+str(lift_method)+'_lift.npy'] traj_files = ['trajectories/traj1'+'_x'+str(box_position[0])+'_y'+str(box_position[1])+'_Y'+str(box_yaw)+'_m'+str(reach_method)+'_reach.npy'] traj_rep = TrajectoryReproducer(traj_files) default_joint_stiffness = np.array([5000.0, 5000.0, 8000.0, 5000.0, 5000.0, 300.0, 2000.0, 300.0, 5000.0, 8000.0, 5000.0, 5000.0, 300.0, 2000.0, 300.0]) default_joint_damping = np.array([30.0, 30.0, 50.0, 30.0, 30.0, 1.0, 5.0, 1.0, 30.0, 50.0, 30.0, 30.0, 1.0, 5.0, 1.0]) # ROBOT MODEL for trying ID robot_urdf = '/home/domingo/robotology-superbuild/configs/ADVR_shared/centauro/urdf/centauro_upper_body_updated_inertia.urdf' robot_model = RobotModel(robot_urdf) #LH_name = 'LWrMot3' #RH_name = 'RWrMot3' joint_pos_state = np.zeros(robot_model.q_size) joint_vel_state = np.zeros(robot_model.qdot_size) joint_effort_state = np.zeros(robot_model.qdot_size) joint_effort_ref_state = np.zeros(robot_model.qdot_size) joint_stiffness_state = np.zeros(robot_model.qdot_size) joint_damping_state = np.zeros(robot_model.qdot_size) joint_state_id = [] def callback(data, params): joint_ids = params[0] joint_pos_state = params[1]
# 50, 80, 50, 50, 10, 20, 300]) #Kd_tau = np.array([1.60, 1.00, 1.60, 1.00, 1.00, 0.40, # 1.60, 1.00, 1.00, 1.00, 1.00, 0.40, # 1.00, 1.60, 1.00, # 1.00, 1.60, 1.00, 1.00, 0.20, 0.40, 0.00, # 0.06, 0.06, # 1.00, 1.60, 1.00, 1.00, 0.20, 0.40, 0.00]) #Kd_tau = default_joint_damping #Kp_tau = 100 * default_joint_stiffness/1000 #Kd_tau = 2 * default_joint_damping/100 # ROBOT MODEL for trying ID robot_urdf_file = '/home/domingo/robotology-superbuild/robots/iit-bigman-ros-pkg/bigman_urdf/urdf/bigman.urdf' robot_urdf_file = '/home/domingo/robotology-superbuild/configs/ADVR_shared/bigman/urdf/bigman.urdf' robot_model = RobotModel(robot_urdf_file) #LH_name = 'LWrMot3' #RH_name = 'RWrMot3' joint_pos_state = np.zeros(robot_model.q_size) joint_vel_state = np.zeros(robot_model.qdot_size) joint_effort_state = np.zeros(robot_model.qdot_size) joint_stiffness_state = np.zeros(robot_model.qdot_size) joint_damping_state = np.zeros(robot_model.qdot_size) joint_state_id = [] def callback(data, params): joint_ids = params[0] joint_pos_state = params[1] joint_effort_state = params[2]
drill_x = 0.70 drill_y = 0.00 drill_z = 0.0184 drill_yaw = 0 # Degrees drill_size = [0.1, 0.1, 0.3] drill_relative_pose = create_drill_relative_pose(drill_x=drill_x, drill_y=drill_y, drill_z=drill_z, drill_yaw=drill_yaw) drill_relative_pose = create_drill_relative_pose(drill_x=drill_x+0.02, drill_y=drill_y+0.02, drill_z=drill_z, drill_yaw=drill_yaw+5) final_left_hand_pose = create_hand_relative_pose(drill_relative_pose, hand_x=0, hand_y=drill_size[1]/2, hand_z=0, hand_yaw=0) final_right_hand_pose = create_hand_relative_pose(drill_relative_pose, hand_x=0, hand_y=-drill_size[1]/2, hand_z=0, hand_yaw=0) # ROBOT MODEL for trying ID # robot_urdf_file = os.environ["ROBOTOLOGY_ROOT"]+'/configs/ADVR_shared/bigman/urdf/bigman.urdf' robot_urdf_file = os.environ["ROBOTOLOGY_ROOT"]+'/robots/iit-bigman-ros-pkg/bigman_urdf/urdf/bigman.urdf' robot_rbdl_model = rbdl.loadModel(robot_urdf_file, verbose=False, floating_base=False) robot_model = RobotModel(robot_urdf_file=robot_urdf_file) LH_name = 'LWrMot3' RH_name = 'RWrMot3' torso_name = 'DWYTorso' l_soft_hand_offset = np.array([0.000, -0.030, -0.210]) r_soft_hand_offset = np.array([0.000, 0.030, -0.210]) torso_offset = np.array([0.000, 0.000, 0.000]) # Stiffness/Damping gains from Xbot config file default_joint_stiffness = np.array([8000., 5000., 8000., 5000., 5000., 2000., 8000., 5000., 5000., 5000., 5000., 2000., 5000., 8000., 5000., 5000., 8000., 5000., 5000., 300., 2000., 300., 300., 300., 5000., 8000., 5000., 5000., 300., 2000., 300.]) default_joint_damping = np.array([30., 50., 30., 30., 30., 5.,
drill_y = 0.00 drill_z = -0.1327 drill_yaw = 0 # Degrees #drill_size = [0.1, 0.1, 0.3] drill_size = [0.11, 0.11, 0.3] # Beer final_drill_height = 0.0 drill_relative_pose = create_drill_relative_pose(drill_x=drill_x, drill_y=drill_y, drill_z=drill_z, drill_yaw=drill_yaw) # Robot Model (It is used to calculate the IK cost) #robot_urdf_file = os.environ["ROBOTOLOGY_ROOT"]+'/configs/ADVR_shared/bigman/urdf/bigman.urdf' robot_urdf_file = os.environ[ "ROBOTOLOGY_ROOT"] + '/robots/iit-bigman-ros-pkg/bigman_urdf/urdf/bigman.urdf' robot_model = RobotModel(robot_urdf_file) LH_name = 'LWrMot3' RH_name = 'RWrMot3' l_soft_hand_offset = np.array([0.000, -0.030, -0.210]) r_soft_hand_offset = np.array([0.000, 0.030, -0.210]) touching_drill_config = np.array([ 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.0568, 0.2386, -0.2337, -1.6803, 0.2226, 0.0107, 0.5633, 0., 0., 0.0568, -0.2386, 0.2337, -1.6803, -0.2226, 0.0107, -0.5633 ]) # ################### # # ################### # # ### ENVIRONMENT ### # # ################### #
] traj_rep = TrajectoryReproducer(traj_files) default_joint_stiffness = np.array([ 8000., 5000., 8000., 5000., 5000., 2000., 8000., 5000., 5000., 5000., 5000., 2000., 5000., 8000., 5000., 5000., 8000., 5000., 5000., 300., 2000., 300., 300., 300., 5000., 8000., 5000., 5000., 300., 2000., 300. ]) default_joint_damping = np.array([ 30., 50., 30., 30., 30., 5., 30., 50., 30., 30., 30., 5., 30., 50., 30., 30., 50., 30., 30., 1., 5., 1., 1., 1., 30., 50., 30., 30., 1., 5., 1. ]) # ROBOT MODEL for trying ID robot_urdf = '/home/domingo/robotology-superbuild/robots/iit-bigman-ros-pkg/bigman_urdf/urdf/bigman.urdf' robot_model = RobotModel(robot_urdf) #LH_name = 'LWrMot3' #RH_name = 'RWrMot3' joint_pos_state = np.zeros(robot_model.q_size) joint_vel_state = np.zeros(robot_model.qdot_size) joint_effort_state = np.zeros(robot_model.qdot_size) joint_stiffness_state = np.zeros(robot_model.qdot_size) joint_damping_state = np.zeros(robot_model.qdot_size) joint_state_id = [] def callback(data, params): joint_ids = params[0] joint_pos_state = params[1] joint_effort_state = params[2]