def compute_forces(q_actual_traj, q_eq_traj, torque_traj, rel_stiffness_list):
firenze = hr.M3HrlRobot(connect=False)
d_gains_list_mN_deg_sec = [-100, -120, -15, -25, -1.25]
d_gains_list = [
180. / 1000. * s / math.pi for s in d_gains_list_mN_deg_sec
]
stiff_list_mNm_deg = [1800, 1300, 350, 600, 60]
stiff_list_Nm_rad = [
180. / 1000. * s / math.pi for s in stiff_list_mNm_deg
]
# stiffness_settings = [0.15,0.7,0.8,0.8,0.8]
# dia = np.array(stiffness_settings) * np.array(stiff_list_Nm_rad)
dia = np.array(rel_stiffness_list) * np.array(stiff_list_Nm_rad)
k_q = np.matrix(np.diag(dia))
dia_inv = 1. / dia
k_q_inv = np.matrix(np.diag(dia_inv))
actual_cart = joint_to_cartesian(q_actual_traj)
eq_cart = joint_to_cartesian(q_eq_traj)
force_traj_jacinv = ForceTrajectory()
force_traj_stiff = ForceTrajectory()
force_traj_torque = ForceTrajectory()
k_cart_list = []
for q_actual, q_dot, q_eq, actual_pos, eq_pos, t, tau_m in zip(
q_actual_traj.q_list, q_actual_traj.qdot_list, q_eq_traj.q_list,
actual_cart.p_list, eq_cart.p_list, q_actual_traj.time_list,
torque_traj.q_list):
q_eq = firenze.clamp_to_physical_joint_limits('right_arm', q_eq)
q_delta = np.matrix(q_actual).T - np.matrix(q_eq).T
tau = k_q * q_delta[0:5, 0] - np.matrix(
np.array(d_gains_list) * np.array(q_dot)[0:5]).T
x_delta = np.matrix(actual_pos).T - np.matrix(eq_pos).T
jac_full = firenze.Jac('right_arm', q_actual)
jac = jac_full[0:3, 0:5]
jac_full_eq = firenze.Jac('right_arm', q_eq)
jac_eq = jac_full_eq[0:3, 0:5]
k_cart = np.linalg.inv(
(jac_eq * k_q_inv *
jac_eq.T)) # calculating stiff matrix using Jacobian for eq pt.
k_cart_list.append(k_cart)
pseudo_inv_jac = np.linalg.inv(jac_full * jac_full.T) * jac_full
tau_full = np.row_stack((tau, np.matrix(tau_m[5:7]).T))
#force = (-1*pseudo_inv_jac*tau_full)[0:3]
force = -1 * pseudo_inv_jac[0:3, 0:5] * tau
force_traj_jacinv.f_list.append(force.A1.tolist())
force_traj_stiff.f_list.append((k_cart * x_delta).A1.tolist())
force_traj_torque.f_list.append(
(pseudo_inv_jac * np.matrix(tau_m).T)[0:3].A1.tolist())
return force_traj_jacinv, force_traj_stiff, force_traj_torque, k_cart_list
def test_elbow_angle():
firenze = hr.M3HrlRobot(connect=False)
rot_mat = tr.Rz(math.radians(-90.))*tr.Ry(math.radians(-90))
x_list = [0.55,0.45,0.35]
y = -0.2
z = -0.23
for x in x_list:
p0 = np.matrix([x,y,z]).T
q = firenze.IK('right_arm',p0,rot_mat)
# q[1] = math.radians(15)
# q = firenze.IK('right_arm',p0,rot_mat,q_guess = q)
elbow_angle = firenze.elbow_plane_angle('right_arm',q)
print '---------------------------------------'
print 'ee position:', p0.A1
# print 'joint angles:', q
print 'elbow_angle:', math.degrees(elbow_angle)
def joint_to_cartesian(traj):
firenze = hr.M3HrlRobot(connect=False)
pts = []
cart_vel = []
for i in range(len(traj.q_list)):
q = traj.q_list[i]
p = firenze.FK('right_arm', q)
pts.append(p.A1.tolist())
if traj.qdot_list != []:
qdot = traj.qdot_list[i]
jac = firenze.Jac('right_arm', q)
vel = jac * np.matrix(qdot).T
cart_vel.append(vel.A1[0:3].tolist())
ct = CartesianTajectory()
ct.time_list = copy.copy(traj.time_list)
ct.p_list = copy.copy(pts)
ct.v_list = copy.copy(cart_vel)
#return np.matrix(pts).T
return ct
def __init__(self):
# stiffness in Nm/rad: [20,50,15,25]
self.settings_r = hr.MekaArmSettings(
stiffness_list=[0.1939, 0.6713, 0.748, 0.7272, 0.8])
# self.settings_r = hr.MekaArmSettings(stiffness_list=[0.2,1.0,1.0,0.4,0.8])
self.settings_stiff = hr.MekaArmSettings(
stiffness_list=[0.8, 1.0, 1.0, 1.0, 0.8])
self.firenze = hr.M3HrlRobot(connect=True,
right_arm_settings=self.settings_stiff)
self.hok = hs.Hokuyo('utm', 0, flip=True)
self.thok = ths.tilt_hokuyo('/dev/robot/servo0',
5,
self.hok,
l1=0.,
l2=-0.055)
self.cam = camera.Camera('mekabotUTM')
self.set_camera_settings()
self.fit_circle_lock = RLock()
threading.Thread.__init__(self)
def __init__(self, move_segway=False, use_right_arm=True,
use_left_arm=True, set_wrist_theta_gc=False, end_effector_length=0.12818):
self.mech_kinematics_lock = RLock()
self.fit_circle_lock = RLock()
if use_right_arm:
# stiffness in Nm/rad: [20,50,15,25,2.5]
#settings_r = hr.MekaArmSettings(stiffness_list=[0.1939,0.6713,0.748,0.7272,0.75])
# stiffness in Nm/rad: [20,50,20,25,2.5]
settings_r = hr.MekaArmSettings(stiffness_list=[0.1939,0.6713,0.997,0.7272,0.75])
#settings_r = hr.MekaArmSettings(stiffness_list=[0.7,0.7,0.9,0.9,0.7]) # for RIP project
else:
settings_r = None
if use_left_arm:
if set_wrist_theta_gc:
settings_l = hr.MekaArmSettings(stiffness_list=[0.1939,0.6713,0.748,0.7272,.8,.3,1.],
control_mode = 'wrist_theta_gc')
else:
settings_l = hr.MekaArmSettings(stiffness_list=[0.1939,0.6713,0.748,0.7272,0.75])
print 'left arm assigned'
#settings_l = hr.MekaArmSettings(stiffness_list=[0.7,0.7,0.9,0.9,0.7]) # for RIP project
else:
settings_l = None
print 'left arm NOT assigned'
self.firenze = hr.M3HrlRobot(connect = True, left_arm_settings = settings_l,
right_arm_settings = settings_r, end_effector_length = end_effector_length)
rospy.init_node('compliant_trajectory', anonymous = False)
rospy.Subscriber('mechanism_kinematics_rot',
MechanismKinematicsRot,
self.mechanism_kinematics_rot_cb)
rospy.Subscriber('mechanism_kinematics_jac',
MechanismKinematicsJac,
self.mechanism_kinematics_jac_cb)
self.mech_traj_pub = rospy.Publisher('mechanism_trajectory', Point32)
self.hok = hs.Hokuyo('utm', 0, flip = True, ros_init_node = False)
self.thok = ths.tilt_hokuyo('/dev/robot/servo0',5,self.hok,l1=0.,l2=-0.055)
self.cam = camera.Camera('mekabotUTM')
self.set_camera_settings()
self.z = zenither.Zenither(robot='HRL2', pose_read_thread=True)
self.zenither_client = zc.ZenitherClient()
self.zenither_list = []
self.zenither_moving = False
self.move_segway_flag = move_segway
self.segway_trajectory = at.PlanarTajectory()
self.move_segway_lock = RLock()
self.segway_motion_tup = (0.0,0.0,0.0)
self.new_segway_command = False
self.dist_boundary = 0.05
self.eq_pt_close_to_bndry = False # used in segway_motion_local
self.workspace_dict = ut.load_pickle(os.environ['HRLBASEPATH']+'/src/projects/equilibrium_point_control/pkls/workspace_dict_2010Feb20_225427.pkl')
if move_segway:
self.segway_command_node = sc.SegwayCommand(vel_topic='/hrl/segway/command',
pose_local_topic='/hrl/segway/pose_local',
pose_global_topic='/hrl/segway/pose_global',
stop_topic='/hrl/segway/stop',
max_vel_topic='/hrl/segway/max_vel',
ros_init_node=False)
#self.segway_pose = vop.vo_pose('/hrl/segway/pose',ros_init_node=False)
self.segway_pose = vop.vo_pose('pose',ros_init_node=False)
self.eq_pt_not_moving_counter = 0
if ha == None:
raise RuntimeError('You need to specify a hooking angle (--ha)')
#p = np.matrix([0.45,-0.2,-0.23]).T
p = np.matrix([0.55,-0.2,-0.23]).T
if opt.la:
arm = 'left_arm'
p[1,0] = p[1,0] * -1
settings_l = hr.MekaArmSettings(stiffness_list=[0.15,0.7,0.8,0.8,0.8])
settings_r = None
else:
arm = 'right_arm'
settings_l = None
settings_r = hr.MekaArmSettings(stiffness_list=[0.15,0.7,0.8,0.8,0.8])
firenze = hr.M3HrlRobot(connect = True, right_arm_settings = settings_r,
left_arm_settings = settings_l)
print 'hit a key to power up the arms.'
k=m3t.get_keystroke()
firenze.power_on()
print 'hit a key to test IK'
k=m3t.get_keystroke()
rot_mat = tr.Rz(math.radians(ha))*tr.Ry(math.radians(-90))
firenze.go_cartesian(arm, p, rot_mat, speed=0.1)
if test_ik_flag:
test_IK(arm, p, rot_mat)
print 'hit a key to end everything'