#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]