def main(): 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(200) 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 #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()
def main(): try: rospy.init_node('avalos_limb_py') limb = Limb() print "Joint Name:" print limb.joint_names() print "\n" + "Joint Angles:" print limb.joint_angles() print "\n" + "Joint Velocities:" print limb.joint_velocities() print "\n" + "Endpoint Pose:" print limb.endpoint_pose() print "\n" + "Endpoint Velocity:" print limb.endpoint_velocity() except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
def main(): rospy.init_node('testing_node') limb = Limb('right') print('yay') # r = rospy.Rate(1000) # limb.set_command_timeout(.1) wrench = limb.endpoint_effort() force = wrench['force'] mag = force_mag(force) force_2d_dir = normalize( proj(force, normalize([1, 1, 0])) * normalize([force[0], force[1], 0])) rotation_matrix = np.matrix([[np.cos(np.pi / 2), -np.sin(np.pi / 2), 0], [np.sin(np.pi / 2), np.cos(np.pi / 2), 0], [0, 0, 1]]) force_base_frame = rotation_matrix * np.reshape(force_2d_dir, (3, 1)) force_base_frame[0] = -force_base_frame[0] force_base_frame = np.array(force_base_frame).flatten() print("force_base_frame: ", force_base_frame) print("force_base_frame: ", type(force_base_frame)) # print(force_2d_dir) cur_pos = limb.endpoint_pose().get('position') print("force: ", force) print("force 2d dir", type(force_2d_dir))
def main(): rospy.init_node("get_arm_pose") arm = Limb() tf_listener = tf.TransformListener() armpose = arm.endpoint_pose() quat_arr = armpose['orientation'] print("position is {}".format(armpose['position'])) print("\n") print("orientation is {}".format(quat_arr)) # euler_angles = euler_from_quaternion(quat_arr, 'sxyz') # print("The euler angles are {}".format(euler_angles)) #joints = arm.joint_angles() #print(joints) # rate = rospy.Rate(10.0) # while not rospy.is_shutdown(): # try: # # (controller_pos, controller_quat) = listener.lookupTransform('world', 'controller', rospy.Time(0)) # (pos, quat) = tf_listener.lookupTransform('world', 'reference/right_hand', rospy.Time(0)) # print("right hand orientation is {}".format(euler_from_quaternion(quat, 'sxyz'))) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # continue # rate.sleep() rospy.spin()
def attach_springs(self): """ Switches to joint torque mode and attached joint springs to current joint positions. """ # record initial joint angles self._start_angles = self._limb.joint_angles() # set control rate control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and return to Position Control Mode self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) limb = Limb() kin = sawyer_kinematics('right') pose = limb.endpoint_pose() point = pose.get('position') quaternion = pose.get('orientation') # loop at specified rate commanding new joint torques while not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces(limb, kin, point, quaternion) control_rate.sleep()
def sendIOT(): #rospy.init_node('avalos_limb_py') while not rospy.is_shutdown(): limb = Limb() j_p=limb.joint_angles() j_v=limb.joint_velocities() p_p=limb.endpoint_pose() v_p=limb.endpoint_velocity() if running_status: joint_p = {'right_j0': j_p['right_j0'], 'right_j1': j_p['right_j1'], 'right_j2': j_p['right_j2'], 'right_j3': j_p['right_j3'], 'right_j4': j_p['right_j4'], 'right_j5': j_p['right_j5'], 'right_j6': j_p['right_j6']} joint_v = {'right_j0': j_v['right_j0'], 'right_j1': j_v['right_j1'], 'right_j2': j_v['right_j2'], 'right_j3': j_v['right_j3'], 'right_j4': j_v['right_j4'], 'right_j5': j_v['right_j5'], 'right_j6': j_v['right_j6']} success = device_client.publishEvent( "joint_angle", "json", {'j_p': joint_p,'j_v': joint_v}, qos=0, on_publish=my_on_publish_callback()) #print data time.sleep(0.1) if not success: print("Not connected to WatsonIoTP")
def main(): rospy.init_node("parry") global arm global listener arm = Limb() clash_pub = rospy.Publisher('clash', Float64, queue_size=1) twist_sub = rospy.Subscriber('/vive/twist1', TwistStamped, twist_callback, queue_size=1) listener = tf.TransformListener() old_tracker_pose = 0 # set the end effector twist sword_twist = Twist() arm_twist = Twist() no_twist = Twist() no_twist.linear.x, no_twist.linear.y, no_twist.linear.z = 0, 0, 0 no_twist.angular.x, no_twist.angular.y, no_twist.angular.z = 0, 0, 0 rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: # (controller_pos, controller_quat) = listener.lookupTransform('world', 'controller', rospy.Time(0)) (tracker_pos, tracker_quat) = listener.lookupTransform('world', 'tracker', rospy.Time(0)) # get sword pose (sword_position, _) = listener.lookupTransform('world', 'sword_tip', rospy.Time(0)) # get arm position armpose = arm.endpoint_pose() arm_position = armpose['position'] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue tracker_pos = np.array(tracker_pos) arm_position = np.array(arm_position) sword_position = np.array(sword_position) #offset tracker position so that robot sword meets at mid point tracker_pos[2] += sword_zoffset # displacement = tracker_pos - sword_position # set velocity in the direction of the displacement disp_mag = np.linalg.norm(displacement) clash_pub.publish(disp_mag) tracker_pos_mag = np.linalg.norm(tracker_pos) # print("distance between tracker and world {}".format(tracker_pos_mag)) # print("The distance between the arm and tracker is {}".format(disp_mag)) #print("distance is {}".format(tracker_pos_mag)) arm_twist.linear.x = 0.30 * displacement[0] / disp_mag arm_twist.linear.y = 0.30 * displacement[1] / disp_mag arm_twist.linear.z = 0.30 * displacement[2] / disp_mag arm_twist.angular.x = 0.3 * np.random.choice(end_effector_directions) arm_twist.angular.y = 0 arm_twist.angular.z = 0 pos_diff = np.linalg.norm(old_tracker_pose) - tracker_pos_mag print("tracker twist is {}".format(tracker_twist)) # if user sword is less than 1.25m to robot # and distance between robot arm and user sword is less than 0.15m # and user sword is moving towards robot... if tracker_pos_mag < 1.25 and disp_mag > 0.07: #pass move(arm_twist) if tracker_twist < 0.15: arm.set_joint_positions(arm.joint_angles()) elif tracker_pos_mag > 1.25: #pass arm.set_joint_positions(home_config) else: #pass move(no_twist) old_tracker_pose = tracker_pos_mag rate.sleep()
class Fencer(): def __init__(self): self.Blist = sw.Blist self.M = sw.M self.Slist = mr.Adjoint(self.M).dot(self.Blist) self.eomg = 0.01 # positive tolerance on end-effector orientation error self.ev = 0.001 # positive tolerance on end-effector position error self.arm = None self.listener = None self.home_config = { 'right_j6': -1.3186796875, 'right_j5': 0.5414912109375, 'right_j4': 2.9682451171875, 'right_j3': 1.7662939453125, 'right_j2': -3.0350302734375, 'right_j1': 1.1202939453125, 'right_j0': -0.0001572265625 } self.tracker_position = None self.arm_position = None self.sword_position = None self.sword_zoffset = 0.25 self.end_effector_directions = [-1.0, 1.0] self.tracker_twist = 50 self.end_effector_vel = np.zeros(6) # velocities need to be passed in as a dictionary self.joint_vels_dict = {} # set the end effector twist self.sword_twist = Twist() self.arm_twist = Twist() self.no_twist = Twist() self.no_twist.linear.x = 0 self.no_twist.linear.y = 0 self.no_twist.linear.z = 0 self.no_twist.angular.x = 0 self.no_twist.angular.y = 0 self.no_twist.angular.z = 0 self.arm = Limb() self.clash_pub = rospy.Publisher('clash', Float64, queue_size=1) self.twist_sub = rospy.Subscriber('/vive/twist1', TwistStamped, self.twist_callback, queue_size=1) self.tf_listener = tf.TransformListener() self.rate = rospy.Rate(10.0) def move(self, twist_msg): # have to switch the order of linear and angular velocities in twist # message so that it comes in the form needed by the modern_robotics library self.end_effector_vel[0] = 0 #twist_msg.angular.x self.end_effector_vel[1] = 0 #twist_msg.angular.y self.end_effector_vel[2] = 0 #twist_msg.angular.z self.end_effector_vel[3] = twist_msg.linear.x self.end_effector_vel[4] = twist_msg.linear.y self.end_effector_vel[5] = twist_msg.linear.z arm_angles_dict = self.arm.joint_angles() thetalist0 = [] for i in range(len(arm_angles_dict)): thetalist0.append(arm_angles_dict['right_j' + str(i)]) J = mr.JacobianSpace(self.Slist, np.array(thetalist0)) pinv_J = np.linalg.pinv(J) # print("The shape of the end effector velocity vector is {}".format(self.end_effector_vel.shape)) # print("The shape of the Jacobian Pseudo Inverse matrix is {}".format(pinv_J.shape)) joint_vels = np.dot(pinv_J, self.end_effector_vel) for i, vel in enumerate(joint_vels): self.joint_vels_dict['right_j' + str(i)] = vel # set joint velocities self.arm.set_joint_velocities(self.joint_vels_dict) def twist_callback(self, msg): # get magnitude of tracker twist self.tracker_twist = np.linalg.norm([msg.twist.linear.x, \ msg.twist.linear.y, \ msg.twist.linear.z]) def fence(self): old_tracker_pose = 0 while not rospy.is_shutdown(): try: # (controller_pos, controller_quat) = listener.lookupTransform('world', 'controller', rospy.Time(0)) (self.tracker_position, _) = self.tf_listener.lookupTransform('world', 'tracker', rospy.Time(0)) # get sword pose (self.sword_position, _) = self.tf_listener.lookupTransform('world', 'sword_tip', rospy.Time(0)) # get arm position armpose = self.arm.endpoint_pose() self.arm_position = armpose['position'] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue self.tracker_position = np.array(self.tracker_position) self.arm_position = np.array(self.arm_position) self.sword_position = np.array(self.sword_position) #offset tracker position so that robot sword meets at mid point self.tracker_position[2] += self.sword_zoffset # displacement = self.tracker_position - self.sword_position # set velocity in the direction of the displacement disp_mag = np.linalg.norm(displacement) self.clash_pub.publish(disp_mag) tracker_pos_mag = np.linalg.norm(self.tracker_position) # print("distance between tracker and world {}".format(tracker_pos_mag)) # print("The distance between the arm and tracker is {}".format(disp_mag)) #print("distance is {}".format(tracker_pos_mag)) self.arm_twist.linear.x = 0.30 * displacement[0] / disp_mag self.arm_twist.linear.y = 0.30 * displacement[1] / disp_mag self.arm_twist.linear.z = 0.30 * displacement[2] / disp_mag self.arm_twist.angular.x = 0.3 * np.random.choice( self.end_effector_directions) self.arm_twist.angular.y = 0 self.arm_twist.angular.z = 0 pos_diff = np.linalg.norm(old_tracker_pose) - tracker_pos_mag print("tracker twist is {}".format(self.tracker_twist)) # if user sword is less than 1.25m to robot # and distance between robot arm and user sword is less than 0.15m # and user sword is moving towards robot... if tracker_pos_mag < 1.25 and disp_mag > 0.07: #pass self.move(self.arm_twist) if self.tracker_twist < 0.15: self.arm.set_joint_positions(self.arm.joint_angles()) elif tracker_pos_mag > 1.25: #pass self.arm.set_joint_positions(self.home_config) else: #pass self.move(self.no_twist) old_tracker_pose = tracker_pos_mag self.rate.sleep()
class Fencer(): def __init__(self): self.Blist = sw.Blist self.M = sw.M self.Slist = mr.Adjoint(self.M).dot(self.Blist) self.eomg = 0.01 # positive tolerance on end-effector orientation error self.ev = 0.001 # positive tolerance on end-effector position error self.arm = None self.listener = None self.home_config = { 'right_j6': -1.3186796875, 'right_j5': 0.5414912109375, 'right_j4': 2.9682451171875, 'right_j3': 1.7662939453125, 'right_j2': -3.0350302734375, 'right_j1': 1.1202939453125, 'right_j0': -0.0001572265625 } self.tracker_position = None self.tracker_pos_mag = None self.arm_position = None self.sword_position = None self.attack_position = None self.tag_position = None self.disp_mag = None self.attack_disp_mag = None self.fence_region = 1.50 self.fence_speed = 0.50 self.sword_zoffset = 0.25 self.end_effector_directions = [-1.0, 1.0] self.tracker_twist = 50 self.end_effector_vel = np.zeros(6) # velocities need to be passed in as a dictionary self.joint_vels_dict = {} # set the end effector twist self.sword_twist = Twist() self.arm_twist = Twist() self.no_twist = Twist() self.no_twist.linear.x = 0 self.no_twist.linear.y = 0 self.no_twist.linear.z = 0 self.no_twist.angular.x = 0 self.no_twist.angular.y = 0 self.no_twist.angular.z = 0 self.arm = Limb() print("going to home configuration") self.arm.set_joint_positions(self.home_config) self.clash_pub = rospy.Publisher('clash', Float64, queue_size=1) self.twist_sub = rospy.Subscriber('/vive/twist1', TwistStamped, self.twist_callback, queue_size=1) self.tf_listener = tf.TransformListener() self.rate = rospy.Rate(10.0) def move(self, twist_msg): # have to switch the order of linear and angular velocities in twist # message so that it comes in the form needed by the modern_robotics library self.end_effector_vel[0] = 0 #twist_msg.angular.x self.end_effector_vel[1] = 0 #twist_msg.angular.y self.end_effector_vel[2] = 0 #twist_msg.angular.z self.end_effector_vel[3] = twist_msg.linear.x self.end_effector_vel[4] = twist_msg.linear.y self.end_effector_vel[5] = twist_msg.linear.z arm_angles_dict = self.arm.joint_angles() thetalist0 = [] for i in range(len(arm_angles_dict)): thetalist0.append(arm_angles_dict['right_j' + str(i)]) J = mr.JacobianSpace(self.Slist, np.array(thetalist0)) pinv_J = np.linalg.pinv(J) # print("The shape of the end effector velocity vector is {}".format(self.end_effector_vel.shape)) # print("The shape of the Jacobian Pseudo Inverse matrix is {}".format(pinv_J.shape)) joint_vels = np.dot(pinv_J, self.end_effector_vel) for i, vel in enumerate(joint_vels): self.joint_vels_dict['right_j' + str(i)] = vel # set joint velocities self.arm.set_joint_velocities(self.joint_vels_dict) def twist_callback(self, msg): # get magnitude of tracker twist self.tracker_twist = np.linalg.norm([msg.twist.linear.x, \ msg.twist.linear.y, \ msg.twist.linear.z]) def fence(self): while not rospy.is_shutdown(): try: # (controller_pos, controller_quat) = listener.lookupTransform('world', 'controller', rospy.Time(0)) (self.tracker_position, _) = self.tf_listener.lookupTransform('world', 'tracker', rospy.Time(0)) # get sword pose (self.sword_position, _) = self.tf_listener.lookupTransform('world', 'sword_tip', rospy.Time(0)) # get tag position # (self.tag_position, _) = self.tf_listener.lookupTransform('world', 'ar_marker_0', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): self.tracker_position = None self.sword_position = None self.tag_position = None self.arm.set_joint_positions(self.home_config) continue # get arm position armpose = self.arm.endpoint_pose() self.arm_position = armpose['position'] self.tracker_position = np.array(self.tracker_position) self.arm_position = np.array(self.arm_position) self.sword_position = np.array(self.sword_position) self.tag_position = np.array(self.tag_position) #offset tracker position so that robot sword meets at mid point self.tracker_position[2] += self.sword_zoffset print("tracker twist is {}".format(self.tracker_twist)) if self.tracker_twist > 0.20: # user moving sword self.defend() else: self.attack() self.rate.sleep() def defend(self): if self.tracker_position is None: self.arm.set_joint_positions(self.home_config) return else: displacement = self.tracker_position - self.sword_position # set velocity in the direction of the displacement self.disp_mag = np.linalg.norm(displacement) self.clash_pub.publish(self.disp_mag) self.tracker_pos_mag = np.linalg.norm(self.tracker_position) # print("distance between tracker and world {}".format(tracker_pos_mag)) self.arm_twist.linear.x = self.fence_speed * displacement[ 0] / self.disp_mag self.arm_twist.linear.y = self.fence_speed * displacement[ 1] / self.disp_mag self.arm_twist.linear.z = self.fence_speed * displacement[ 2] / self.disp_mag self.arm_twist.angular.x = self.fence_speed * np.random.choice( self.end_effector_directions) self.arm_twist.angular.y = 0 self.arm_twist.angular.z = 0 if self.tracker_pos_mag < self.fence_region and self.disp_mag > 0.07: #pass self.move(self.arm_twist) if self.disp_mag < 0.40: self.arm.set_joint_positions(self.arm.joint_angles()) time.sleep(1) elif self.tracker_pos_mag > self.fence_region: self.arm.set_joint_positions(self.home_config) else: #pass self.move(self.no_twist) def attack(self): #self.arm.set_joint_positions(self.home_config) # define new attack point x_point = np.random.uniform(low=self.tracker_position[0] + 0.10, high=self.tracker_position[0] + 0.10) y_point = np.random.uniform(low=self.tracker_position[1] - 0.50, high=self.tracker_position[1]) z_point = np.random.uniform(low=self.tracker_position[2], high=self.tracker_position[2] + 0.40) self.attack_position = np.array([x_point, y_point, z_point]) # if self.tag_position is None: # self.arm.set_joint_positions(self.home_config) # return # else: # displacement = self.tag_position - self.arm_position displacement = self.attack_position - self.arm_position # set velocity in the direction of the displacement self.attack_disp_mag = np.linalg.norm(displacement) self.clash_pub.publish(self.disp_mag) self.arm_twist.linear.x = self.fence_speed * displacement[ 0] / self.attack_disp_mag self.arm_twist.linear.y = self.fence_speed * displacement[ 1] / self.attack_disp_mag self.arm_twist.linear.z = self.fence_speed * displacement[ 2] / self.attack_disp_mag self.arm_twist.angular.x = 0 self.arm_twist.angular.y = self.fence_speed * np.random.choice( self.end_effector_directions) self.arm_twist.angular.z = 0 self.move(self.arm_twist) if self.disp_mag < 0.40: self.arm.set_joint_positions(self.arm.joint_angles()) time.sleep(1) elif self.tracker_pos_mag > self.fence_region: self.arm.set_joint_positions(self.home_config) else: #pass #self.move(self.no_twist) self.arm.set_joint_positions(self.home_config)