def main(): rospy.init_node('testing_node') limb = Limb('right') print('yay') r = rospy.Rate(200) kin = sawyer_kinematics('right') Jt = kin.jacobian_transpose() F = np.array([0, 0, 0, 0, 0, 10]) tau = np.dot(Jt, F) # print(tau) # rospy.sleep(20) # limb.set_command_timeout(.1) # wrench = limb.endpoint_effort() # force = wrench['force'] # print("force type: ", force) # print(wrench) while not rospy.is_shutdown(): # joint_torques = np.array([[0,0,0,0,0,0,0]]) Jt = kin.jacobian_transpose() mcart = kin.cart_inertia() f = np.array([0, 0, -9.8, 0, 0, 0]) gravity = np.dot(np.dot(Jt, mcart), f) joint_torques = np.array([ 1.5474950095623006, -16.78679653980678, -2.8277487768926406, -0.1771867794616826, 0.1073210015442511, -0.5216893350253217, -0.00607477942479895 ]) # cur = limb.joint_efforts() # print('set joints as:', joint_torques[0]) # set_torques = joint_array_to_dict(joint_torques[0], limb) # print(set_torques) # print(set_torques.get('right_j1')) # torque = np.array([gravity.item(i) for i in range(7)]) torque = joint_array_to_dict(joint_torques, limb) # cur['right_j1'] = 0 # cur['right_j2'] = 0 print('joint torques', torque) limb.set_joint_torques(torque) r.sleep()
def main(): print("Getting robot state... ") self._rs = intera_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() print('sup') rospy.init_node('sawyer_kinematics') print('init') # rospy.sleep(10) # init node???? # step 1 determine initial point # step 2 determine desired endpoint # in loop now # step 3 measure force and position # step 4 determine force we need to apply to push towards desired endpoint # step 5 convert force to joint torques with Jacobian # step 6 command joint torques # end of loop r = rospy.Rate(1000) limb = Limb() kin = sawyer_kinematics('right') #step 1 pose = limb.endpoint_pose() point = pose.get('position') quaternion = pose.get('orientation') # both are 3x #step 2 path = np.array([.1, 0, 0]) des_point = point + path des_quaternion = quaternion path_dir = normalize(path) #step 3 cur_point = point error = np.linalg.norm(des_point - cur_point) tol = .01 K = 1 #stiffness alpha = 1 #multiplier while not rospy.is_shutdown() and error > tol: #step 3 force = get_end_force(limb) # print("force is:", force) f_mag = force_mag(force) f_dir = force_dir(force) #step 4 f_right = proj(path, force) * path_dir f_wrong = force - f_right force_apply = f_right * alpha - f_wrong * K force_apply = np.hstack((force_apply, np.array([0, 0, 0]))) # print('force_apply is:', force_apply) #step 5 Jt = kin.jacobian_transpose() joint_torques = np.dot(Jt, force_apply) #make sure right dims joint_torques = np.asarray(joint_torques) joint_torques = np.array([[0, 0, 0, 0, 0, 0, 0]]) # print('set joints as:', joint_torques[0]) set_torques = joint_array_to_dict(joint_torques[0], limb) print(set_torques) print() #step 6 limb.set_joint_torques(set_torques) cur_point = limb.endpoint_pose().get('position') error = np.linalg.norm(des_point - cur_point) r.sleep()