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()
