def __init__(self): self.t = time.time() self.__AUTOCAMERA_MODE__ = self.MODE.simulation self.autocamera = Autocamera() # Instantiate the Autocamera Class self.jnt_msg = JointState() self.joint_angles = {'ecm':None, 'psm1':None, 'psm2':None} self.cam_info = {'left':CameraInfo(), 'right':CameraInfo()} self.last_ecm_jnt_pos = None self.first_run = True # For forward and inverse kinematics self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name) self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name) self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description') self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name) self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description') self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name) # For camera clutch control self.camera_clutch_pressed = False self.ecm_manual_control_lock_mtml_msg = None self.ecm_manual_control_lock_ecm_msg = None self.mtml_start_position = None self.mtml_end_position = None self.initialize_psms_initialized = 30 self.__DEBUG_GRAPHICS__ = False
def execute_move(self, pos): rospy.loginfo('moving') # Read in pose data. Adjust the height to be above the block and the length so that the laser sees the table instead of the block pos.position.z += .1 pos.position.x += .005 q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z] p =[[pos.position.x],[pos.position.y],[pos.position.z]] # Convert quaternion data to rotation matrix R = quat_to_so3(q); # Form transformation matrix robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') # Create seed with current position q0 = kdl_kin.random_joint_angles() limb_interface = baxter_interface.limb.Limb('right') limb_interface.set_joint_position_speed(.3) current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()] for ind in range(len(q0)): q0[ind] = current_angles[ind] pose = kdl_kin.forward(q0) pose[0:3,0:3] = R pose[0:3,3] = p # Solve for joint angles, iterating if no solution is found seed = 0.3 q_ik = kdl_kin.inverse(pose, q0+seed) while q_ik == None: seed += 0.3 q_ik = kdl_kin.inverse(pose, q0+seed) rospy.loginfo(q_ik) # Calculate the joint trajectory with a quintic time scaling q_list = JointTrajectory(q0,q_ik,1,50,5) # Iterate through list for q in q_list: # Format joint angles as limb joint angle assignment angles = limb_interface.joint_angles() for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] rospy.sleep(.07) # Send joint move command limb_interface.set_joint_positions(angles) # Publish state and hand position rospy.sleep(1) rospy.loginfo(4) self._pub_state.publish(4) rospy.loginfo(pos) self._pub_hand.publish(pos) self._done = True print('Done')
def __init__(self): self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name) self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name) self.psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description') self.psm2_kin = KDLKinematics(self.psm2_robot, self.psm2_robot.links[0].name, self.psm2_robot.links[-1].name) self.zoom_level_positions = {'l1':None, 'r1':None, 'l2':None, 'r2':None, 'lm':None, 'rm':None} self.logerror("autocamera_initialized")
def move_to(pos): pub_demo_state = rospy.Publisher('demo_state',Int16, queue_size = 10) if (pos == 1): catch = np.array([.65, .2, 0]) # my .7? R = np.array([[ 0.26397895, -0.34002068, 0.90260791], [-0.01747676, -0.93733484, -0.34799134], [ 0.96437009, 0.07608772, -0.25337913]]) elif (pos == 2): catch = np.array([.68, -.05, 0]) R = np.array([[0,0,1],[0,-1,0],[1,0,0]]) elif (pos == 3): catch = np.array([.72, .1, 0]) R = np.array([[ 0.26397895, -0.34002068, 0.90260791], [-0.01747676, -0.93733484, -0.34799134], [ 0.96437009, 0.07608772, -0.25337913]]) else: pass th_init = np.array([-.3048, -.2703, -.1848, 1.908, .758, -1.234, -3.04]) X = RpToTrans(R,catch) # Find end joint angles with IK robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'left_gripper_base') seed = 0 q_ik = kdl_kin.inverse(X, th_init) while q_ik == None: seed += 0.01 q_ik = kdl_kin.inverse(X, th_init+seed) if (seed>1): # return False break q_goal = q_ik print q_goal limb_interface = baxter_interface.limb.Limb('left') angles = limb_interface.joint_angles() for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q_goal[ind] limb_interface.move_to_joint_positions(angles) # rospy.sleep(5) print 'done' pub_demo_state.publish(1)
def __init__(self, robot, base_link, end_link, group, move_group_ns="move_group", planning_scene_topic="planning_scene", robot_ns="", verbose=False, kdl_kin=None, closed_form_IK_solver = None, joint_names=[]): self.robot = robot self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if kdl_kin is None: self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) else: self.kdl_kin = kdl_kin self.base_link = base_link self.joint_names = joint_names self.end_link = end_link self.group = group self.robot_ns = robot_ns self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction) self.acceleration_magnification = 1 rospy.wait_for_service('compute_cartesian_path') self.cartesian_path_plan = rospy.ServiceProxy('compute_cartesian_path',GetCartesianPath) self.verbose = verbose self.closed_form_IK_solver = closed_form_IK_solver
def __init__(self, sim=True): f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r') robot = URDF.from_xml_string(f.read()) # parsed URDF # robot = URDF.from_parameter_server() self.kin = KDLKinematics(robot, 'base', 'mconn') # Selection of end-effector parameters for WidowX # x, y, z, yaw, pitch self.cart_from_matrix = lambda T: np.array([ T[0, 3], T[1, 3], T[2, 3], math.atan2(T[1, 0], T[0, 0]), -math.asin(T[2, 0])]) self.home = np.array([0.05, 0, 0.25, 0, 0]) # home end-effector pose self.q = np.array([0, 0, 0, 0, 0]) # joint angles self.dt = 0.05 # algorithm time step self.sim = sim # true if virtual robot def publish(eventStop): # for arbotix pub1 = rospy.Publisher("q1/command", Float64, queue_size=5) pub2 = rospy.Publisher("q2/command", Float64, queue_size=5) pub3 = rospy.Publisher("q3/command", Float64, queue_size=5) pub4 = rospy.Publisher("q4/command", Float64, queue_size=5) pub5 = rospy.Publisher("q5/command", Float64, queue_size=5) # for visualization jointPub = rospy.Publisher( "/joint_states", JointState, queue_size=5) jmsg = JointState() jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5'] while not rospy.is_shutdown() and not eventStop.is_set(): jmsg.header.stamp = rospy.Time.now() jmsg.position = self.q if self.sim: jointPub.publish(jmsg) pub1.publish(self.q[0]) pub2.publish(self.q[1]) pub3.publish(self.q[2]) pub4.publish(self.q[3]) pub5.publish(self.q[4]) eventStop.wait(self.dt) rospy.init_node("robot") eventStop = threading.Event() threadJPub = threading.Thread(target=publish, args=(eventStop,)) threadJPub.daemon = True threadJPub.start() # Update joint angles in a background process
def execute_cb(self,goal): self.moving_to_new_x_pos = False self.moving_to_new_y_pos = False self.stop_base_movement = False self.max_virtual_x_vel = 0.05 self.max_virtual_y_vel = 0.05 self.commanded_virtual_x_pos = 0.0 self.commanded_virtual_y_pos = 0.0 self.commanded_virtual_x_vel = 0.0 self.commanded_virtual_y_vel = 0.0 self.virtual_x_cmd_time_sec = 0.0 self.virtual_y_cmd_time_sec = 0.0 self.virtual_x_running_time_sec = 0.0 self.virtual_y_running_time_sec = 0.0 youbot_urdf = URDF.from_parameter_server() self.kin_with_virtual = KDLKinematics(youbot_urdf, "virtual", "gripper_palm_link") self.kin_grasp = KDLKinematics(youbot_urdf, "base_link", "gripper_palm_link") # Create a timer that will be used to monitor the velocity of the virtual # joints when we need them to be positioning themselves. self.vel_monitor_timer = rospy.Timer(rospy.Duration(0.1), self.vel_monitor_timer_callback) self.arm_joint_pub = rospy.Publisher("arm_1/arm_controller/position_command",JointPositions,queue_size=10) self.gripper_pub = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions, queue_size=10) self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) # Give the publishers time to get setup before trying to do any actual work. rospy.sleep(2) # Initialize at the candle position. self.publish_arm_joint_positions(armJointPosCandle) rospy.sleep(2.0) #self.publish_arm_joint_positions(armJointPos1) #rospy.sleep(3.0) #self.publish_arm_joint_positions(armJointPos2) #rospy.sleep(10.0) #self.publish_arm_joint_positions(armJointPosCandle) #rospy.sleep(2.0) # Go through the routine of picking up the block. self.grasp_routine() self._result.successOrNot=1 self._as.set_succeeded(self._result)
def find_everything_related_to_world(arm_name, xyzrpy): global psm1_kin,psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot if len(xyzrpy) > 2: xyzrpy = [ tuple(xyzrpy[0:3]), tuple(xyzrpy[3:])] psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name) Twp1 = psm1_kin.forward([]) if arm_name == 'psm2': Tp12 = pose_converter.PoseConv.to_homo_mat(xyzrpy) Twp2 = Twp1 * Tp12 Twp2_euler = pose_converter.PoseConv.to_pos_euler(Twp2) return Twp2_euler if arm_name == 'ecm': ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[1].name, ecm_robot.links[3].name) Tse = ecm_kin.forward([]) Tp1E = pose_converter.PoseConv.to_homo_mat(xyzrpy) Tws = Twp1 * Tp1E * (Tse**-1) Tws_euler = pose_converter.PoseConv.to_pos_euler(Tws) return Tws_euler
def kdl_kinematics (self,data): self.q_sensors = data self.tree = kdl_tree_from_urdf_model(self.omni) # create a kdl tree from omni URDF model self.omni_kin = KDLKinematics(self.omni, "base", "stylus") # create instance that captures the kinematics of the robot arm #Forward Kinematics self.pose_stylus = self.omni_kin.forward(data) #compute the forward kinematics from the sensor joint angle position using the kinematics from the kdl tree #Inverse Kinematics self.q_guess = numpy.array(data)+0.2 #make an initial guess for your joint angles by deviating all the sensor joint angles by 0.2 self.q_ik = self.omni_kin.inverse(self.pose_stylus, self.q_guess) #using the position from the forward kinematics 'pose_stylus' and the offset initial guess, compute #the desired joint angles for that position. self.delta_q = self.q_ik-data
def __init__(self): """Read robot describtion""" self.robot_ = URDF.from_parameter_server() self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector') self.cur_pose_ = None self.cur_jnt_ = None # Creates the SimpleActionClient, passing the type of the action self.client_ = actionlib.SimpleActionClient('/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction) self.client_.wait_for_server() self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal() #time_from_start=rospy.Duration(10) self.goal_.trajectory.joint_names=['j1','j2','j3','j4','j5','j6'] #To be reached 1 second after starting along the trajectory trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint() trajectory_point.positions=[0]*6 trajectory_point.time_from_start=rospy.Duration(0.1) self.goal_.trajectory.points.append(trajectory_point) rospy.Subscriber('joint_states', sensor_msgs.msg.JointState, self.fwd_kinematics_cb) rospy.Subscriber('command', geometry_msgs.msg.Pose, self.inv_kinematics_cb)
def __init__(self): # For forward and inverse kinematics self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name) self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name) self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description') self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name) self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description') self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name) # For camera clutch control self.camera_clutch_pressed = False self.ecm_manual_control_lock_mtml_msg = None self.ecm_manual_control_lock_ecm_msg = None self.mtml_start_position = None self.mtml_end_position = None self.ecm_msg = None self.__clutchNGo_mode__ = self.MODE.simulation self.autocamera = Autocamera()
def main(): a = rospy.init_node('set_base_frames') global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot, mtmr_robot, mtmr_kin if psm1_robot is None: psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name) if psm2_robot is None: psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description') psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name) if ecm_robot is None: ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name) ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name) if mtmr_robot is None: mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description') mtmr_base = KDLKinematics(mtmr_robot, mtmr_robot.links[0].name, mtmr_robot.links[1].name) # pdb.set_trace() psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, latch=True, queue_size=1) psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, latch=True, queue_size=1) mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, latch=True, queue_size=1) ecm_tip = ecm_kin.forward([1,1,1,1]) # ECM Tool Tip ecm_tip = np.linalg.inv(ecm_tip) psm1_base_frame = psm1_kin.forward([]) psm1_message = pose_converter.PoseConv.to_pose_msg(psm1_base_frame) psm2_base_frame = psm2_kin.forward([]) psm2_message = pose_converter.PoseConv.to_pose_msg(psm2_base_frame) psm1_pub.publish(psm1_message) psm2_pub.publish(psm2_message) # mtmr_pub.publish(message) print (psm1_message) print (psm2_message) sleep (1)
def find_path(plot, pos): # Set initial position q_start = np.array([ -0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914, -1.16889336, -0.90888362 ]) limb_interface = baxter_interface.limb.Limb('right') angles = limb_interface.joint_angles() for ind, joint in enumerate(limb_interface.joint_names()): q_start[ind] = angles[joint] print q_start # Find goal for throwing if pos == 1: catch_y = .7 elif pos == 2: catch_y = .1 elif pos == 3: catch_y = .3 pos_goal = find_feasible_release(catch_x, catch_y, catch_z, pos) block_width = .047 throw_y = pos_goal[0] throw_z = pos_goal[1] dy = catch_y - throw_y - block_width vel = pos_goal[2] alpha = pos_goal[3] t = np.linspace(0, dy / (vel * cos(alpha)), 100) traj_y = vel * cos(alpha) * t + throw_y traj_z = -.5 * 9.8 * t**2 + vel * sin(alpha) * t + throw_z if (plot == True): plt.close('all') plt.figure() plt.hold(False) plt.plot(traj_y, traj_z, 'r', linewidth=2) plt.hold(True) plt.plot(traj_y[0], traj_z[0], 'r.', markersize=15) plt.ylim([-.8, .5]) plt.xlim([-.2, .8]) plt.xlabel('Y position (m)') plt.ylabel('Z position (m)') plt.title('Desired trajectory') plt.show(block=False) wm = plt.get_current_fig_manager() wm.window.wm_geometry("800x500+1000+0") # wm.window.setGeometry(800,500,0,0) raw_input("Press enter to continue...") # plt.show(block=False) print('found release state') print pos_goal # Add rotation to position and convert to rotation matrix R = np.array([[0.11121663, -0.14382586, 0.98333361], [-0.95290138, -0.2963578, 0.06442835], [0.28215212, -0.94418546, -0.17001177]]) p = np.hstack((0.68, pos_goal[0:2])) X = RpToTrans(R, p) # Find end joint angles with IK robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') thInit = q_start seed = 0 q_ik = kdl_kin.inverse(X, thInit) while q_ik == None: seed += 0.01 q_ik = kdl_kin.inverse(X, thInit + seed) if (seed > 1): # return False break q_goal = q_ik # print q_goal # Transform to joint velocities using Jacobian jacobian = kdl_kin.jacobian(q_ik) inv_jac = np.linalg.pinv(jacobian) Vb = np.array([0, 0, 0, 0, pos_goal[2], pos_goal[3]]) q_dot_goal = inv_jac.dot(Vb) iter = 1000 treeA = np.empty((iter + 1, 14)) edgesA = np.empty((iter, 2)) treeB = np.empty((iter + 1, 14)) edgesB = np.empty((iter, 2)) treeA[0] = np.hstack((q_start, np.array([0, 0, 0, 0, 0, 0, 0]))) treeB[0] = np.hstack((q_goal, q_dot_goal.tolist()[0])) treeA, treeB, edgesA, edgesB, indA, indB = build_tree( iter, treeA, treeB, edgesA, edgesB, plot, kdl_kin) np.linalg.norm(q_start - q_goal) pathA = np.empty((edgesA.shape[0], 14)) pathA[0] = treeA[indA] curEdge = edgesA[indA - 1] ind = 1 atOrigin = False while atOrigin == False: pathA[ind] = treeA[curEdge[0]] curEdge = edgesA[curEdge[0] - 1] ind += 1 if curEdge[0] == 0: atOrigin = True pathA[ind] = treeA[curEdge[0]] pathA = pathA[0:ind + 1, :] pathB = np.empty((edgesB.shape[0], 14)) pathB[0] = treeB[indB] curEdge = edgesB[indB - 1] ind = 1 atOrigin = False # print treeB, pathB while atOrigin == False: pathB[ind] = treeB[curEdge[0]] curEdge = edgesB[curEdge[0] - 1] if curEdge[0] == 0: atOrigin = True # ind += 1 else: ind += 1 pathB[ind] = treeB[curEdge[0]] pathB = pathB[0:ind + 1, :] path = np.vstack((pathA[::-1], pathB)) if (plot): stepList = np.empty((path.shape[0], 3)) for i in range(path.shape[0]): stepList[i] = kdl_kin.forward(path[i, :7])[0:3, 3].transpose() plt.plot(stepList[:, 1], stepList[:, 2], 'g', linewidth=2) plt.show(block=False) raw_input('Press enter to continue...') plt.close('all') # if (plot): # plt.figure() # plt.plot(path[:,0:7],'.') # plt.show(block=False) # print(path.shape) return path
class JTCartesianController(object): def __init__(self): limb = read_parameter('~limb', 'right') if limb not in ['right', 'left']: rospy.logerr('Unknown limb name [%s]' % limb) return # Read the controllers parameters gains = read_parameter('~gains', dict()) self.kp = joint_list_to_kdl(gains['Kp']) self.kd = joint_list_to_kdl(gains['Kd']) self.publish_rate = read_parameter('~publish_rate', 500) self.frame_id = read_parameter('~frame_id', 'base') self.timeout = read_parameter('~timeout', 0.02) # seconds # Set-up baxter interface self.arm_interface = baxter_interface.Limb(limb) # set_joint_torques must be commanded at a rate great than the timeout specified by set_command_timeout. # If the timeout is exceeded before a new set_joint_velocities command is received, the controller will switch # modes back to position mode for safety purposes. self.arm_interface.set_command_timeout(self.timeout) # Baxter kinematics self.urdf = URDF.from_parameter_server(key='robot_description') self.tip_link = '%s_gripper' % limb self.kinematics = KDLKinematics(self.urdf, self.frame_id, self.tip_link) self.fk_vel_solver = PyKDL.ChainFkSolverVel_recursive( self.kinematics.chain) # Adjust the publish rate of baxter's state joint_state_pub_rate = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16) # Get the joint names and limits self.joint_names = self.arm_interface.joint_names() self.num_joints = len(self.joint_names) self.lower_limits = np.zeros(self.num_joints) self.upper_limits = np.zeros(self.num_joints) # KDL joint may be in a different order than expected kdl_lower_limits, kdl_upper_limits = self.kinematics.get_joint_limits() for i, name in enumerate(self.kinematics.get_joint_names()): if name in self.joint_names: idx = self.joint_names.index(name) self.lower_limits[idx] = kdl_lower_limits[i] self.upper_limits[idx] = kdl_upper_limits[i] self.middle_values = (self.upper_limits + self.lower_limits) / 2.0 # Move the arm to a neutral position #~ self.arm_interface.move_to_neutral(timeout=10.0) #~ neutral_pos = [-math.pi/4, 0.0, 0.0, 0.0, 0.0, math.pi/2, -math.pi/2] neutral_pos = [1.0, -0.57, -1.2, 1.3, 0.86, 1.4, -0.776] #~ neutral_pos = [pi/4, -pi/3, 0.0, pi/2, 0.0, pi/3, 0.0] neutral_cmd = dict() for i, name in enumerate(self.joint_names): if name in ['left_s0', 'left_w2']: neutral_cmd[name] = neutral_pos[i] * -1.0 else: neutral_cmd[name] = neutral_pos[i] self.arm_interface.move_to_joint_positions(neutral_cmd, timeout=10.0) # Baxter faces self.limb = limb self.face = FaceImage() self.face.set_image('happy') # Set-up publishers/subscribers self.suppress_grav_comp = rospy.Publisher( '/robot/limb/%s/suppress_gravity_compensation' % limb, Empty) joint_state_pub_rate.publish(int(self.publish_rate)) self.feedback_pub = rospy.Publisher('/takktile/force_feedback', Wrench) rospy.Subscriber('/baxter/%s_ik_command' % limb, PoseStamped, self.ik_command_cb) self.gripper_closed = False rospy.Subscriber('/takktile/calibrated', Touch, self.takktile_cb) rospy.Subscriber('/robot/end_effector/%s_gripper/state' % limb, EndEffectorState, self.end_effector_cb) rospy.loginfo('Running Cartesian controller for the %s arm' % limb) # Start torque controller timer self.cart_command = None self.torque_controller_timer = rospy.Timer( rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb) # Shutdown hookup to clean-up before killing the script rospy.on_shutdown(self.shutdown) def end_effector_cb(self, msg): self.gripper_closed = msg.position < 80.0 def get_joint_angles_array(self): out = np.zeros(self.num_joints) joint_angles = self.arm_interface.joint_angles() for i, name in enumerate(self.joint_names): out[i] = joint_angles[name] return out def ik_command_cb(self, msg): if self.cart_command == None: self.face.set_image('thinking_%s' % self.limb) self.cart_command = msg # Add stamp because the console app doesn't use it self.cart_command.header.stamp = rospy.Time.now() def shutdown(self): self.face.set_image('indifferent') # This order mathers! self.arm_interface.exit_control_mode() self.torque_controller_timer.shutdown() def takktile_cb(self, msg): if self.gripper_closed: y = 0 else: y = 0.003 * (sum(msg.pressure[0:4]) - sum(msg.pressure[6:10])) z = 0.006 * (msg.pressure[4] + msg.pressure[10]) # Changes the reference frame of the force vector T = baxter_to_kdl_frame(self.arm_interface.endpoint_pose()) force = T.M * PyKDL.Vector(0, y, z) # Populate the wrench message wrench = Wrench() wrench.force.x = force.x() wrench.force.y = force.y() wrench.force.z = force.z() try: self.feedback_pub.publish(wrench) except: pass def torque_controller_cb(self, event): if rospy.is_shutdown() or self.cart_command == None: return elapsed_time = rospy.Time.now() - self.cart_command.header.stamp if elapsed_time.to_sec() > self.timeout: return # TODO: Validate msg.header.frame_id ## Cartesian error to zero using a Jacobian transpose controller x_target = posemath.fromMsg(self.cart_command.pose) q = self.get_joint_angles_array() x = baxter_to_kdl_frame(self.arm_interface.endpoint_pose()) xdot = baxter_to_kdl_twist(self.arm_interface.endpoint_velocity()) # Calculate a Cartesian restoring wrench x_error = PyKDL.diff(x_target, x) wrench = np.matrix(np.zeros(6)).T for i in range(len(wrench)): wrench[i] = -(self.kp[i] * x_error[i] + self.kd[i] * xdot[i]) # Calculate the jacobian J = self.kinematics.jacobian(q) # Convert the force into a set of joint torques. tau = J^T * wrench tau = J.T * wrench # Populate the joint_torques in baxter API format (dictionary) joint_torques = dict() for i, name in enumerate(self.joint_names): joint_torques[name] = tau[i] self.arm_interface.set_joint_torques(joint_torques)
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link): rospy.loginfo("Creating {} instance".format(self.__class__.__name__)) self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None) if self.openmeta_testbench_manifest_path is not None: self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path) # TestBench Parameters self._params = {} for tb_param in self._testbench_manifest['Parameters']: self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type # TestBench Metrics self._metrics = {} for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO... self._metrics[tb_metric['Name']] = tb_metric['Value'] if hebi_gains is None: hebi_gains = { 'p': [float(self._params['p_gain'])]*3, 'i': [float(self._params['i_gain'])]*3, 'd': [float(self._params['d_gain'])]*3 } hebi_families = [] hebi_names = [] for actuator in hebi_mapping: family, name = actuator.split('/') hebi_families.append(family) hebi_names.append(name) rospy.loginfo(" hebi_group_name: %s", hebi_group_name) rospy.loginfo(" hebi_families: %s", hebi_families) rospy.loginfo(" hebi_names: %s", hebi_names) self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names) # Set PID Gains cmd_msg = CommandMsg() cmd_msg.name = hebi_mapping cmd_msg.settings.name = hebi_mapping cmd_msg.settings.control_strategy = [4, 4, 4] cmd_msg.settings.position_gains.name = hebi_mapping cmd_msg.settings.position_gains.kp = hebi_gains['p'] cmd_msg.settings.position_gains.ki = hebi_gains['i'] cmd_msg.settings.position_gains.kd = hebi_gains['d'] cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me. self.hebi_wrap.send_command_with_acknowledgement(cmd_msg) if base_link is None or end_link is None: robot = Robot.from_xml_string(urdf_str) if not base_link: base_link = robot.get_root() # WARNING: There may be multiple leaf nodes if not end_link: end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0] # pykdl self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link) self._active_joints = self.kdl_fk.get_joint_names() # joint data self.position_fbk = [0.0]*self.hebi_wrap.hebi_count self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count # joint state publisher while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping): rospy.sleep(0.1) self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self.hebi_wrap.add_feedback_callback(self._joint_state_cb) # Set up Waypoint/Trajectory objects self.start_wp = WaypointMsg() self.start_wp.names = self.hebi_wrap.hebi_mapping self.end_wp = copy.deepcopy(self.start_wp) self.goal = TrajectoryGoal() self.goal.waypoints = [self.start_wp, self.end_wp] self._hold_positions = [0.0]*3 self._hold = True self.jointstate = JointState() self.jointstate.name = self.hebi_wrap.hebi_mapping rospy.sleep(1.0)
def __init__(self, base_link, end_link, planning_group, world="/world", listener=None, broadcaster=None, traj_step_t=0.1, max_acc=1, max_vel=1, max_goal_diff=0.02, goal_rotation_weight=0.01, max_q_diff=1e-6, start_js_cb=True, base_steps=10, steps_per_meter=100, closed_form_IK_solver=None, dof=7, perception_ns="/SPServer"): self.world = world self.base_link = base_link self.end_link = end_link self.planning_group = planning_group self.dof = dof self.base_steps = base_steps self.steps_per_meter = steps_per_meter self.MAX_ACC = max_acc self.MAX_VEL = max_vel self.traj_step_t = traj_step_t self.max_goal_diff = max_goal_diff self.max_q_diff = max_q_diff self.goal_rotation_weight = goal_rotation_weight self.at_goal = True self.near_goal = True self.moving = False self.q0 = None #[0] * self.dof self.old_q0 = [0] * self.dof self.cur_stamp = 0 # Set up TF broadcaster if not broadcaster is None: self.broadcaster = broadcaster else: self.broadcaster = tf.TransformBroadcaster() # Set up TF listener and smartmove manager if listener is None: self.listener = tf.TransformListener() else: self.listener = listener # Currently this class does not need a smart waypoint manager. # That will remain in the CoSTAR BT. #self.smartmove_manager = SmartWaypointManager( # listener=self.listener, # broadcaster=self.broadcaster) # TODO: ensure the manager is set up properly # Note that while the waypoint manager is currently a part of CostarArm # If we wanted to set this up for multiple robots it should be treated # as an independent component. self.waypoint_manager = WaypointManager(service=True, broadcaster=self.broadcaster) # Set up services # The CostarArm services let the UI put it into teach mode or anything else self.teach_mode = rospy.Service('/costar/SetTeachMode', SetTeachMode, self.set_teach_mode_call) self.servo_mode = rospy.Service('/costar/SetServoMode', SetServoMode, self.set_servo_mode_call) self.shutdown = rospy.Service('/costar/ShutdownArm', EmptyService, self.shutdown_arm_call) self.servo = rospy.Service('/costar/ServoToPose', ServoToPose, self.servo_to_pose_call) self.plan = rospy.Service('/costar/PlanToPose', ServoToPose, self.plan_to_pose_call) self.smartmove = rospy.Service('/costar/SmartMove', SmartMove, self.smart_move_call) self.js_servo = rospy.Service('/costar/ServoToJointState', ServoToJointState, self.servo_to_joints_call) self.save_frame = rospy.Service('/costar/SaveFrame', SaveFrame, self.save_frame_call) self.save_joints = rospy.Service('/costar/SaveJointPosition', SaveFrame, self.save_joints_call) self.get_waypoints_srv = GetWaypointsService(world=world, service=False, ns=perception_ns) self.driver_status = 'IDLE' # Create publishers. These will send necessary information out about the state of the robot. self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd', JointTrajectoryPoint, queue_size=1000) self.status_publisher = rospy.Publisher('/costar/DriverStatus', String, queue_size=1000) self.display_pub = rospy.Publisher('costar/display_trajectory', DisplayTrajectory, queue_size=1000) self.robot = URDF.from_parameter_server() if start_js_cb: self.js_subscriber = rospy.Subscriber('joint_states', JointState, self.js_cb) self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) # cCreate reference to pyKDL kinematics self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) #self.set_goal(self.q0) self.goal = None self.ee_pose = None self.joint_names = [ joint.name for joint in self.robot.joints[:self.dof] ] self.planner = SimplePlanning( self.robot, base_link, end_link, self.planning_group, kdl_kin=self.kdl_kin, joint_names=self.joint_names, closed_form_IK_solver=closed_form_IK_solver)
class URDriver(): MAX_ACC = .5 MAX_VEL = 1.8 JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] def __init__(self): rospy.init_node('ur_simulation',anonymous=True) rospy.logwarn('SIMPLE_UR SIMULATION DRIVER LOADING') # TF self.broadcaster_ = tf.TransformBroadcaster() self.listener_ = tf.TransformListener() # SERVICES self.servo_to_pose_service = rospy.Service('simple_ur_msgs/ServoToPose', ServoToPose, self.servo_to_pose_call) self.set_stop_service = rospy.Service('simple_ur_msgs/SetStop', SetStop, self.set_stop_call) self.set_teach_mode_service = rospy.Service('simple_ur_msgs/SetTeachMode', SetTeachMode, self.set_teach_mode_call) self.set_servo_mode_service = rospy.Service('simple_ur_msgs/SetServoMode', SetServoMode, self.set_servo_mode_call) # PUBLISHERS AND SUBSCRIBERS self.driver_status_publisher = rospy.Publisher('/ur_robot/driver_status',String) self.robot_state_publisher = rospy.Publisher('/ur_robot/robot_state',String) self.joint_state_publisher = rospy.Publisher('joint_states',JointState) # self.follow_pose_subscriber = rospy.Subscriber('/ur_robot/follow_goal',PoseStamped,self.follow_goal_cb) # Predicator self.pub_list = rospy.Publisher('/predicator/input', PredicateList) self.pub_valid = rospy.Publisher('/predicator/valid_input', ValidPredicates) rospy.sleep(1) pval = ValidPredicates() pval.pheader.source = rospy.get_name() pval.predicates = ['moving', 'stopped', 'running'] pval.assignments = ['robot'] self.pub_valid.publish(pval) # Rate self.run_rate = rospy.Rate(100) self.run_rate.sleep() ### Set Up Simulated Robot ### self.driver_status = 'IDLE' self.robot_state = 'POWER OFF' robot = URDF.from_parameter_server() self.kdl_kin = KDLKinematics(robot, 'base_link', 'ee_link') # self.q = self.kdl_kin.random_joint_angles() self.q = [-1.5707,-.785,-3.1415+.785,-1.5707-.785,-1.5707,0] # Start Pose? self.start_pose = self.kdl_kin.forward(self.q) self.F_start = tf_c.fromMatrix(self.start_pose) # rospy.logwarn(self.start_pose) # rospy.logwarn(type(self.start_pose)) # pose = self.kdl_kin.forward(q) # joint_positions = self.kdl_kin.inverse(pose, q+0.3) # inverse kinematics # if joint_positions is not None: # pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose # J = self.kdl_kin.jacobian(q) # rospy.logwarn('q:'+str(q)) # rospy.logwarn('joint_positions:'+str(joint_positions)) # rospy.logwarn('pose:'+str(pose)) # if joint_positions is not None: # rospy.logwarn('pose_sol:'+str(pose_sol)) # rospy.logwarn('J:'+str(J)) ### START LOOP ### while not rospy.is_shutdown(): if self.driver_status == 'TEACH': self.update_from_marker() # if self.driver_status == 'SERVO': # self.update_follow() # Publish and Sleep self.publish_status() self.send_command() self.run_rate.sleep() # Finish rospy.logwarn('SIMPLE UR - Simulation Finished') def update_from_marker(self): try: F_target_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/endpoint_interact',rospy.Time(0))) F_target_base = tf_c.fromTf(self.listener_.lookupTransform('/base_link','/endpoint_interact',rospy.Time(0))) F_base_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/base_link',rospy.Time(0))) self.F_command = F_base_world.Inverse()*F_target_world M_command = tf_c.toMatrix(self.F_command) joint_positions = self.kdl_kin.inverse(M_command, self.q) # inverse kinematics if joint_positions is not None: pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose self.q = joint_positions else: rospy.logwarn('no solution found') # self.send_command() except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: rospy.logwarn(str(e)) def send_command(self): self.current_joint_positions = self.q msg = JointState() msg.header.stamp = rospy.get_rostime() msg.header.frame_id = "simuated_data" msg.name = self.JOINT_NAMES msg.position = self.current_joint_positions msg.velocity = [0]*6 msg.effort = [0]*6 self.joint_state_publisher.publish(msg) pose = self.kdl_kin.forward(self.q) F = tf_c.fromMatrix(pose) # F = self.F_command self.current_tcp_pose = tf_c.toMsg(F) self.current_tcp_frame = F self.broadcaster_.sendTransform(tuple(F.p),tuple(F.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link') def set_teach_mode_call(self,req): if req.enable == True: # self.rob.set_freedrive(True) self.driver_status = 'TEACH' return 'SUCCESS - teach mode enabled' else: # self.rob.set_freedrive(False) self.driver_status = 'IDLE' return 'SUCCESS - teach mode disabled' def set_servo_mode_call(self,req): if req.mode == 'SERVO': self.driver_status = 'SERVO' return 'SUCCESS - servo mode enabled' elif req.mode == 'DISABLE': self.driver_status = 'IDLE' return 'SUCCESS - servo mode disabled' def set_stop_call(self,req): rospy.logwarn('SIMPLE UR - STOPPING ROBOT') self.rob.stop() return 'SUCCESS - stopped robot' def servo_to_pose_call(self,req): if self.driver_status == 'SERVO': rospy.logwarn(req) self.F_command = tf_c.fromMsg(req.target) M_command = tf_c.toMatrix(self.F_command) # Calculate IK joint_positions = self.kdl_kin.inverse(M_command, self.q) # inverse kinematics if joint_positions is not None: pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose self.q = joint_positions else: rospy.logwarn('no solution found') # self.send_command(F_command) return 'SUCCESS - moved to pose' else: rospy.logerr('SIMPLE UR -- Not in servo mode') return 'FAILED - not in servo mode' def publish_status(self): self.driver_status_publisher.publish(String(self.driver_status)) self.robot_state_publisher.publish(String(self.robot_state)) ps = PredicateList() ps.pheader.source = rospy.get_name() ps.statements = [] statement = PredicateStatement( predicate='moving', confidence=1, value=PredicateStatement.FALSE, num_params=1, params=['robot', '', '']) ps.statements += [statement] statement = PredicateStatement( predicate='stopped', confidence=1, value=PredicateStatement.TRUE, num_params=1, params=['robot', '', '']) ps.statements += [statement] statement = PredicateStatement( predicate='running', confidence=1, value=PredicateStatement.TRUE, num_params=1, params=['robot', '', '']) ps.statements += [statement] self.pub_list.publish(ps) def check_robot_state(self): self.robot_state == 'RUNNING SIMULATION' self.driver_status = 'IDLE - SIMULATION'
def execute_move(self, pos): # Close the gripper self._right_gripper.close() self._right_gripper.close() self._right_gripper.close() self._right_gripper.close() self._right_gripper.close() rospy.loginfo('moving') # Get robot parameters and create a limb interface instance robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') limb_interface = baxter_interface.limb.Limb('right') angles = limb_interface.joint_angles() limb_interface.set_joint_position_speed(.7) q_goal = [-.01859, -.5119, 1.7909, 1.232, -1.030, 1.945, -1.31] q0 = kdl_kin.random_joint_angles() current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()] for ind in range(len(q0)): q0[ind] = current_angles[ind] q_list = JointTrajectory(q0,np.asarray(q_goal),1,50,5) for q in q_list: for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] limb_interface.set_joint_positions(angles) rospy.sleep(.03) # Create the desired joint trajectory with a quintic time scaling q0 = q_goal # Set the dropoff position to be dropoff #1 if self._goal == 1: q_goal = [-.675, -.445, 1.626, 1.1336, -1.457, 1.6145, -2.190] # Dropoff #2 else: q_goal = [-.066, -.068, 1.738, .8022, -2.23, .917, -2.9057] q_list = JointTrajectory(np.asarray(q0),np.asarray(q_goal),1,50,5) for q in q_list: for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] limb_interface.set_joint_positions(angles) rospy.sleep(.03) # Open the gripper rospy.sleep(.2) self._right_gripper.open() self._right_gripper.open() self._right_gripper.open() self._right_gripper.open() self._right_gripper.open() # Set the position to the old goal and the new goal to the desired camera position for seeing the blocks q0 = q_goal q_goal = [-.01859, -.5119, 1.7909, 1.232, -1.030, 1.945, -1.31] q_list = JointTrajectory(np.asarray(q0),np.asarray(q_goal),1,50,5) for q in q_list: for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] limb_interface.set_joint_positions(angles) rospy.sleep(.05) rospy.sleep(.2) # Publish next state self._pub_state.publish(1) self._done = True print('Done')
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model from pykdl_utils.kdl_kinematics import KDLKinematics # VERSION CHANGE - http://answers.ros.org/question/197609/how-to-read-a-urdf-in-python-in-indigo/ #robot = URDF.load_from_parameter_server(verbose=False) robot = URDF.from_parameter_server() base_link = robot.get_root() end_link = "link_6" #robot.link_map.keys()[len(robot.link_map)-1] # robot.links[6] print end_link tree = kdl_tree_from_urdf_model(robot) print tree.getNrOfSegments() chain = tree.getChain(base_link, end_link) print chain.getNrOfJoints() kdl_kin = KDLKinematics(robot, base_link, end_link) q = kdl_kin.random_joint_angles() print 'joints:', q pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat) print 'pose:', pose #q_ik = kdl_kin.inverse(pose, q+0.3) # inverse kinematics #if q_ik is not None: # pose_sol = kdl_kin.forward(q_ik) # should equal pose #J = kdl_kin.jacobian(q) #print 'q:', q #print 'q_ik:', q_ik #if q_ik is not None: # print 'pose_sol:', pose_sol #print 'J:', J
class JTCartesianController(object): def __init__(self): # Read the controllers parameters gains = read_parameter('~gains', dict()) self.kp = joint_list_to_kdl(gains['Kp']) self.kd = joint_list_to_kdl(gains['Kd']) self.publish_rate = read_parameter('~publish_rate', 500) self.frame_id = read_parameter('~frame_id', 'base_link') self.tip_link = read_parameter('~tip_link', 'end_effector') # Kinematics self.urdf = URDF.from_parameter_server(key='robot_description') self.kinematics = KDLKinematics(self.urdf, self.frame_id, self.tip_link) # Get the joint names and limits self.joint_names = self.kinematics.get_joint_names() self.num_joints = len(self.joint_names) # Set-up publishers/subscribers self.torque_pub = dict() for i, name in enumerate(self.joint_names): self.torque_pub[name] = rospy.Publisher('/%s/command' % (name), Float64) rospy.Subscriber('/joint_states', JointState, self.joint_states_cb) rospy.Subscriber('/grips/endpoint_state', EndpointState, self.endpoint_state_cb) rospy.Subscriber('/grips/ik_command', PoseStamped, self.ik_command_cb) rospy.loginfo('Running Cartesian controller for Grips') # Start torque controller timer self.cart_command = None self.endpoint_state = None self.joint_states = None self.torque_controller_timer = rospy.Timer( rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb) # Shutdown hookup to clean-up before killing the script rospy.on_shutdown(self.shutdown) def endpoint_state_cb(self, msg): self.endpoint_state = msg def ik_command_cb(self, msg): self.cart_command = msg # Add stamp manually. This is a hack for console commands (rostopic pub) self.cart_command.header.stamp = rospy.Time.now() def joint_states_cb(self, msg): self.joint_states = msg def shutdown(self): self.torque_controller_timer.shutdown() # Stop the torque commands. It avoids unwanted movements after stoping this controller for i, name in enumerate(self.joint_names): self.torque_pub[name].publish(0.0) def torque_controller_cb(self, event): if rospy.is_shutdown() or None in [ self.cart_command, self.endpoint_state, self.joint_states ]: return # TODO: Validate msg.header.frame_id ## Cartesian error to zero using a Jacobian transpose controller x_target = posemath.fromMsg(self.cart_command.pose) q, qd, eff = self.kinematics.extract_joint_state(self.joint_states) x = posemath.fromMsg(self.endpoint_state.pose) xdot = TwistMsgToKDL(self.endpoint_state.twist) # Calculate a Cartesian restoring wrench x_error = PyKDL.diff(x_target, x) wrench = np.matrix(np.zeros(6)).T for i in range(len(wrench)): wrench[i] = -(self.kp[i] * x_error[i] + self.kd[i] * xdot[i]) # Calculate the jacobian J = self.kinematics.jacobian(q) # Convert the force into a set of joint torques. tau = J^T * wrench tau = J.T * wrench # Publish the joint_torques for i, name in enumerate(self.joint_names): self.torque_pub[name].publish(tau[i])
from tf.transformations import quaternion_from_euler import tf, time import tf.transformations as tfm from urdf_parser_py.urdf import URDF from pykdl_utils.kdl_kinematics import KDLKinematics import helper from helper import util import rospy import moveit_msgs from moveit_msgs.srv import GetPositionIK from moveit_msgs.srv import GetPositionIKRequest from moveit_msgs.srv import GetPositionIKResponse #define forward kinematics configuration robot = URDF.from_parameter_server() fk_solver_r = KDLKinematics(robot, "yumi_body", "yumi_tip_r") fk_solver_l = KDLKinematics(robot, "yumi_body", "yumi_tip_l") # fk_solver_r = KDLKinematics(robot, "yumi_body", "yumi_joint_6_r") # fk_solver_l = KDLKinematics(robot, "yumi_body", "yumi_joint_6_l") #define inverse kinematics configuration ik_solver_r = IK("yumi_body", "yumi_tip_r") ik_solver_l = IK("yumi_body", "yumi_tip_l") ik_srv = rospy.ServiceProxy('/compute_ik', GetPositionIK) def joints_from_pose_list(pose_list, seed_both_arms=None): arm_list = ['l', 'r'] dict_ik_pose = {} dict_ik_pose['joints'] = []
class Push(State): def __init__(self, hebiros_wrapper, urdf_str, base_link, end_link): """SMACH State :type hebiros_wrapper: HebirosWrapper :param hebiros_wrapper: HebirosWrapper instance for Leg HEBI group :type urdf_str: str :param urdf_str: Serialized URDF str :type base_link: str :param base_link: Leg base link name in URDF :type end_link: str :param end_link: Leg end link name in URDF """ State.__init__(self, outcomes=['ik_failed', 'success'], input_keys=[ 'prev_joint_pos', 'target_end_link_point', 'execution_time' ], output_keys=['prev_joint_pos', 'active_joints']) self.hebi_wrap = hebiros_wrapper self.urdf_str = urdf_str self.base_link = base_link self.end_link = end_link self.active = False # hardware interface self._hold_leg_position = True self._hold_joint_angles = [] self.hebi_wrap.add_feedback_callback(self._hold_leg_pos_cb) # pykdl self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link) self._active_joints = self.kdl_fk.get_joint_names() # trac-ik self.trac_ik = IK(base_link, end_link, urdf_string=urdf_str, timeout=0.01, epsilon=1e-4, solve_type="Distance") self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01] self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0 ] # NOTE: This implements position-only IK # joint state publisher while not rospy.is_shutdown() and len( self.hebi_wrap.get_joint_positions()) < len( self.hebi_wrap.hebi_mapping): rospy.sleep(0.1) self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self.hebi_wrap.add_feedback_callback(self._joint_state_cb) def enter(self, ud): self._hold_joint_angles = ud.prev_joint_pos self.active = True def execute(self, ud): self.enter(ud) init_wp = WaypointMsg() end_wp = WaypointMsg() eff_prev_target_pos = self.kdl_fk.forward( ud.prev_joint_pos)[:3, 3].reshape(1, 3).tolist()[0] jt_init_pos = self.hebi_wrap.get_joint_positions() eff_init_pos = self.kdl_fk.forward(jt_init_pos)[:3, 3].reshape( 1, 3).tolist()[0] eff_target_pos = [ ud.target_end_link_point.x, ud.target_end_link_point.y, ud.target_end_link_point.z ] # init_wp init_wp.names = self.hebi_wrap.hebi_mapping init_wp.positions = jt_init_pos init_wp.velocities = [0.0] * self.hebi_wrap.hebi_count init_wp.accelerations = [0.0] * self.hebi_wrap.hebi_count # end_wp end_wp.names = self.hebi_wrap.hebi_mapping success, end_wp.positions = self._get_pos_ik( self.trac_ik, init_wp.positions, eff_target_pos, seed_xyz=eff_prev_target_pos) end_wp.velocities = [0.0] * self.hebi_wrap.hebi_count end_wp.accelerations = [0.0] * self.hebi_wrap.hebi_count goal = TrajectoryGoal() goal.waypoints = [init_wp, end_wp] goal.times = [0.0, ud.execution_time] # send goal to trajectory action server self._hold_leg_position = False self.hebi_wrap.trajectory_action_client.send_goal(goal) self.hebi_wrap.trajectory_action_client.wait_for_result() self._hold_leg_position = True ud.prev_joint_pos = end_wp.positions self.exit(ud) return 'success' def exit(self, ud): ud.active_joints = self._active_joints self.active = False def _get_pos_ik(self, ik_solver, seed_angles, target_xyz, target_wxyz=None, seed_xyz=None, recursion_depth_cnt=100): if recursion_depth_cnt < 0: rospy.logdebug("%s FAILURE. Maximum recursion depth reached", self._get_pos_ik.__name__) return False, seed_angles if target_wxyz is None: target_wxyz = [ 1, 0, 0, 0 ] # trak-ik seems a little more stable when given initial pose for pos-only ik target_jt_angles = ik_solver.get_ik( seed_angles, target_xyz[0], target_xyz[1], target_xyz[2], target_wxyz[0], target_wxyz[1], target_wxyz[2], target_wxyz[3], self.ik_pos_xyz_bounds[0], self.ik_pos_xyz_bounds[1], self.ik_pos_xyz_bounds[2], self.ik_pos_wxyz_bounds[0], self.ik_pos_wxyz_bounds[1], self.ik_pos_wxyz_bounds[2]) if target_jt_angles is not None: rospy.logdebug( "%s SUCCESS. Solution: %s to target xyz: %s from seed angles: %s", self._get_pos_ik.__name__, round_list(target_jt_angles, 4), round_list(target_xyz, 4), round_list(seed_angles, 4)) return True, target_jt_angles else: # ik_solver failed if seed_xyz is None: return False, seed_angles else: # binary recursive search target_xyz_new = [(a + b) / 2.0 for a, b in zip(target_xyz, seed_xyz)] success, recursive_jt_angles = self._get_pos_ik( ik_solver, seed_angles, target_xyz_new, target_wxyz, seed_xyz, recursion_depth_cnt - 1) if not success: return False, seed_angles else: return self._get_pos_ik(ik_solver, recursive_jt_angles, target_xyz, target_wxyz, target_xyz_new, recursion_depth_cnt - 1) def _hold_leg_pos_cb(self, msg): if not rospy.is_shutdown() and self.active and self._hold_leg_position: jointstate = JointState() jointstate.name = self.hebi_wrap.hebi_mapping jointstate.position = self._hold_joint_angles jointstate.velocity = [] jointstate.effort = [] self.hebi_wrap.joint_state_publisher.publish(jointstate) def _joint_state_cb(self, msg): if not rospy.is_shutdown() and self.active: jointstate = JointState() jointstate.header.stamp = rospy.Time.now() jointstate.name = self._active_joints jointstate.position = self.hebi_wrap.get_joint_positions() jointstate.velocity = [] jointstate.effort = [] self._joint_state_pub.publish(jointstate)
class dxf_robot_motion: def __init__(self): self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.seam_point_dict = {} self.plan_seam_dict = {} self.speed = 1.0 self.display = True robot_urdf = URDF.from_parameter_server() #robot_urdf = URDF.from_xml_string(urdf_str) self.kdl_kin = KDLKinematics(robot_urdf, "base_link", "tool0") self.move_group = moveit_commander.MoveGroupCommander("GP12_Planner") self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) planning_frame = self.move_group.get_planning_frame() print("============ Planning frame: %s" % planning_frame) self.dxf_file_response_pub = rospy.Publisher('file_response_string', std_msgs.msg.String, queue_size=10) self.plan_response_pub = rospy.Publisher('plan_response_string', std_msgs.msg.String, queue_size=10) self.execute_seam_response_pub = rospy.Publisher( 'execute_seam_response_string', std_msgs.msg.String, queue_size=10) self.plan_and_execute_response_pub = rospy.Publisher( 'plan_and_execute_seam_response_string', std_msgs.msg.String, queue_size=10) self.move_to_stream_start_response_pub = rospy.Publisher( 'move_to_seam_response', std_msgs.msg.String, queue_size=10) #Ros message used to pass dxf file name to node and process dxf file rospy.Subscriber("dxf_file_name", String, self.dxf_grabber_readfile) #Ros message used to plan and view motion before returning rospy.Subscriber("plan_seam_motion", String, self.plan_robot_motion_call) rospy.Subscriber("execute_seam_motion", String, self.execute_robot_motion_call) rospy.Subscriber("plan_and_execute_seam", String, self.plan_and_execute_call) rospy.Subscriber("move_to_seam_start", String, self.move_to_seam_start_call) self.followjointaction = rospy.Publisher( "joint_trajectory_action/goal", control_msgs.msg.FollowJointTrajectoryActionGoal, queue_size=10) def move_to_seam_start_call(self, data): if (self.seam_point_dict.has_key(data.data)): motion_point = self.seam_point_dict[data.data][0][0] sine_val = self.seam_point_dict[data.data][1][0] self.move_to_seam_start(motion_point, sine_val) #print("finished_move") self.move_to_stream_start_response_pub.publish( "Moved to start of seam") print("finished_move") else: self.move_to_stream_start_response_pub.publish( "Seam key does not exist in loaded dictionary") def plan_robot_motion_call(self, data): if (self.seam_point_dict.has_key(data.data)): motion_points = self.seam_point_dict[data.data][0] sine_vals = self.seam_point_dict[data.data][1] self.display = True plan, fraction, waypoints = self.plan_robot_motion( motion_points, sine_vals) if (fraction == 0.0): self.plan_response_pub.publish("Failed to plan") else: self.plan_seam_dict[data.data] = plan self.plan_response_pub.publish("Planning successful") else: self.plan_response_pub.publish( "Planned seam key does not exist in loaded dictionary") def execute_robot_motion_call(self, data): if (self.plan_seam_dict.has_key(data.data)): plan = self.plan_seam_dict[data.data] self.execute_planned_trajectory(plan) self.execute_seam_response_pub.publish("Execution successful") else: self.execute_seam_response_pub.publish( "Could not find plan in dictionary of planned seams, did you publish to plan_seam_motion first?" ) def plan_and_execute_call(self, data): if (self.seam_point_dict.has_key(data.data)): motion_points = self.seam_point_dict[data.data][0] sine_vals = self.seam_point_dict[data.data][1] self.display = False plan, fraction, waypoints = self.plan_robot_motion( motion_points, sine_vals) self.execute_planned_trajectory(plan) self.plan_and_execute_response_pub.publish( "Planning and execution successful") else: self.plan_and_execute_response_pub.publish( "Planned seam key does not exist in loaded dictionary") def distance_calculator(self, start, end): return math.sqrt((start[0] - end[0])**2 + (start[1] - end[1])**2) def display_robot_trajectory(self, plan): display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() display_trajectory.trajectory.append(plan) self.display_trajectory_publisher.publish(display_trajectory) raw_input("Press Enter to continue...") def move_to_home(self): wpose = self.move_group.get_current_pose().pose wpose.position.x = 0.0 wpose.position.y = 0.0 wpose.position.z = 0.0 wpose.orientation.w = 0.0 wpose.orientation.x = 0.0 wpose.orientation.y = 0.0 wpose.orientation.z = 0.0 self.move_group.set_pose_target(wpose) result = self.move_group.go(wait=True) def move_to_seam_start(self, motion_point, sine): pose_goal = Pose() rpy = [math.pi, 0.0, sine - math.pi / 2] print(rpy) R_result = rox.rpy2R(rpy) quat = rox.R2q(R_result) print(quat) pose_goal.orientation.w = 0.0 pose_goal.orientation.x = quat[1] pose_goal.orientation.y = quat[2] pose_goal.orientation.z = 0.0 #20- sets setpoint in middle of dxf file for robot y axis #middle of dxf y axis is 0, so no centering needed here x_val = (20 - motion_point[0]) * 0.0254 y_val = motion_point[1] * 0.0254 pose_goal.position.x = 0.8 + y_val pose_goal.position.y = x_val pose_goal.position.z = 0.3 pose_goal.position.x += 0.1 #self.robot.set_start_state_to_current_state() self.move_group.set_pose_target(pose_goal) result = self.move_group.go(wait=True) def reset_move(self): wpose = self.move_group.get_current_pose().pose wpose.position.x = 0.7 wpose.position.y = 0.56263286 wpose.position.z = 0.3 wpose.orientation.w = -0.000164824011947 wpose.orientation.x = 1.0 wpose.orientation.y = 0.0 wpose.orientation.z = 0.0 self.move_group.set_pose_target(wpose) result = self.move_group.go(wait=True) def execute_consecutive_motions(self, points): i = 0 #wpose = self.move_group.get_current_pose().pose #self.move_group.set_pose_target(wpose) #plan=self.move_group.go(wpose,wait=True) #self.move_group.execute(plan) for i in points: self.move_group.set_pose_target(i) raw_input("Press Enter to continue...") plan = self.move_group.go(wait=True) self.move_group.stop() #self.display_robot_trajectory(plan) #self.move_group.execute(plan) def execute_planned_trajectory(self, plan): #wpose = self.move_group.get_current_pose().pose #self.move_group.set_pose_target(wpose) #time.sleep(0.5) #result=self.move_group.go(wait=True) #self.move_group.stop() #print("plan:") #print(plan) message = rospy.wait_for_message("/robot_status", RobotStatus) joint_state = rospy.wait_for_message("/joint_states", JointState) while (message.in_motion.val == 1): print(message) message = rospy.wait_for_message("/robot_status", RobotStatus) #self.move_group.clear_pose_targets() #print(joint_state) moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state self.move_group.set_start_state(moveit_robot_state) #add check to see if plan start state agrees with start state or consider adding back in getting current pose and appending it to waypoints potentially result = self.move_group.execute(plan, wait=True) #print(result) if (not result): print("not executed") self.execute_seam_response_pub.publish( "Execution Failed initially but retrying") wpose = self.move_group.get_current_pose().pose #print(wpose) wpose.position.z += 0.1 self.move_group.set_pose_target(wpose) result = self.move_group.go(wait=True) plan, fraction, waypoints = self.plan_robot_motion( self.dxf_points, self.sines) self.execute_planned_trajectory(plan) else: self.execute_seam_response_pub.publish("Executed Successfully") #time.sleep(1) def plan_robot_motion(self, dxf_points, sines): self.dxf_points = dxf_points self.sines = sines waypoints = [] message = rospy.wait_for_message("/robot_status", RobotStatus) joint_state = rospy.wait_for_message("/joint_states", JointState) while (message.in_motion.val == 1): message = rospy.wait_for_message("/robot_status", RobotStatus) #self.move_group.clear_pose_targets() print("hello") moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state self.move_group.set_start_state(moveit_robot_state) #waypoints.append(self.move_group.get_current_pose().pose) #wpose = self.move_group.get_current_pose().pose #waypoints.append(wpose) #state = self.robot.get_current_state() #self.move_group.set_start_state(state) for i in range(len(dxf_points)): pose_goal = Pose() rpy = [math.pi, 0.0, sines[i] - math.pi / 2] print(rpy) R_result = rox.rpy2R(rpy) quat = rox.R2q(R_result) print(quat) pose_goal.orientation.w = 0.0 pose_goal.orientation.x = quat[1] pose_goal.orientation.y = quat[2] pose_goal.orientation.z = 0.0 #20- sets setpoint in middle of dxf file for robot y axis #middle of dxf y axis is 0, so no centering needed here x_val = (20 - dxf_points[i][0]) * 0.0254 y_val = dxf_points[i][1] * 0.0254 pose_goal.position.x = 0.8 + y_val pose_goal.position.y = x_val pose_goal.position.z = 0.3 print(pose_goal) waypoints.append(pose_goal) """ euclidean_distances=[] for i in range(1,len(waypoints)): distance=pow(pow(waypoints[i].position.x-waypoints[i-1].position.x,2)+pow(waypoints[i].position.y-waypoints[i-1].position.y,2)+pow(waypoints[i].position.z-waypoints[i-1].position.z,2),0.5) print(distance) euclidean_distances.append(distance) error_code=None """ (plan, fraction) = self.move_group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print(type(plan)) #self.scene.motion_plan_request replan_value = 0 while (replan_value < 3 and fraction < 1.0): print(fraction) (plan, fraction) = self.move_group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold replan_value += 1 print("WARNING Portion of plan failed, replanning") if (replan_value > 2): return 0, 0, 0 #print(len(euclidean_distances)) #print(len(plan.joint_trajectory.points)) #print(waypoints[0]) #print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions)) #print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(3)) #print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(7)) #print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(11)) #print(plan.joint_trajectory.points) total_distance = 0 distances = [] for i in range(1, len(plan.joint_trajectory.points)): old_cartesian = self.kdl_kin.forward( plan.joint_trajectory.points[i - 1].positions) new_cartesian = self.kdl_kin.forward( plan.joint_trajectory.points[i].positions) distance = pow( pow(new_cartesian.item(3) - old_cartesian.item(3), 2) + pow(new_cartesian.item(7) - old_cartesian.item(7), 2) + pow(new_cartesian.item(11) - old_cartesian.item(11), 2), 0.5) distances.append(distance) total_distance += distance #new_time=plan.joint_trajectory.points[i].time_from_start.to_sec()+(distance/self.speed) #old_time=plan.joint_trajectory.points[i].time_from_start.to_sec() #print("new time") #print(new_time) #print(distance/self.speed) #print(old_time) #if(new_time > old_time): # plan.joint_trajectory.points[i].time_from_start.from_sec(new_time) #else: # print("Error, speed faster than possible execution time") print(total_distance) print(distances) total_time = plan.joint_trajectory.points[-1].time_from_start.to_sec() print(total_time) times = [] for i in distances: new_time = (i / total_distance) * total_time times.append(new_time) """ if(new_timestamp > old_timestamp) next_waypoint->time_from_start.fromSec(new_timestamp); else { //ROS_WARN_NAMED("setAvgCartesianSpeed", "Average speed is too fast. Moving as fast as joint constraints allow."); }""" #print point.time_from_start.secs #for i in range(len(plan.joint_trajectory.points)-1): # print("time between points:\n") # print(plan.joint_trajectory.points[i+1].time_from_start.nsecs-plan.joint_trajectory.points[i].time_from_start.nsecs) #for i in range(1,len(plan.joint_trajectory.points)-1): # plan.joint_trajectory.points[i].velocities=[-0.1,0.05,0.11,0.00000001,-0.05,0.2] #plan=self.move_group.retime_trajectory(moveit_robot_state,plan,velocity_scaling_factor=1.0, algorithm="iterative_spline_parameterization") if (self.display): self.display_robot_trajectory(plan) #self.actiongoal= control_msgs.msg.FollowJointTrajectoryActionGoal() self.goal = control_msgs.msg.FollowJointTrajectoryGoal() #self.actiongoal.header=std_msgs.msg.Header() self.goal.trajectory.joint_names = joint_state.name #self.goal.trajectory.points.resize(len(plan.joint_trajectory.points)) traj = Trajectory() time = 0.01 traj.add_point(plan.joint_trajectory.points[0].positions, time) #time=0.0 for i in range(1, len(plan.joint_trajectory.points)): time += times[i - 1] traj.add_point(plan.joint_trajectory.points[i].positions, time) #traj.start() #traj.wait(15.0) """ trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint() trajectory_point.positions=plan.joint_trajectory.points[i].positions trajectory_point.velocities=plan.joint_trajectory.points[i].velocities #trajectory_point.accelerations=plan.joint_trajectory.points[i].accelerations trajectory_point.time_from_start=plan.joint_trajectory.points[i].time_from_start self.goal.trajectory.points.append(trajectory_point) print(len(plan.joint_trajectory.points)) self.actiongoal.goal=self.goal self.followjointaction.publish(self.actiongoal) print("published goal") """ return plan, fraction, waypoints def dxf_grabber_readfile(self, data): filename = data.data dxf = dxfgrabber.readfile(filename) layer_count = len(dxf.layers) self.seam_point_dict = {} #print(layer_count) #print(len(dxf.blocks)) for entity in dxf.entities: print(entity.dxftype) all_blocks = [ entity for entity in dxf.entities if entity.dxftype == 'INSERT' ] all_splines = [ entity for entity in dxf.entities if entity.dxftype == 'SPLINE' ] #print(all_splines) if (len(all_blocks) != 0): test_block = dxf.blocks[all_blocks[0].name] all_polylines = [ entity for entity in test_block if entity.dxftype == 'POLYLINE' ] else: all_polylines = [ entity for entity in dxf.entities if entity.dxftype == 'POLYLINE' ] #print(len(all_polylines)) x = 0 last_in = False removed = [] flag = False vertices = [] sine_vals = [] #with open('eggs.csv', 'w') as csvfile: # spamwriter = csv.writer(csvfile, delimiter=' ', # quotechar='|', quoting=csv.QUOTE_MINIMAL) for i in range(len(all_polylines)): start = all_polylines[i].points[0] end = all_polylines[i].points[-1] for e in range(len(all_polylines)): if (i == e): continue else: if (all_polylines[e].points[0] == start and all_polylines[e].points[-1] == end): #print("identical start found") #print(start) #print(i) #print(e) #if(all_polylines[e].points[-1]==end): #print("identical end found") #print(end) #print(i) #print(e) if (len(all_polylines[i].points) == len( all_polylines[e].points) and not flag): flag = True removed.append(all_polylines[e]) if (len(all_polylines[i].points) > len( all_polylines[e].points)): #print(len(all_polylines[i].points)) #print(len(all_polylines[e].points)) removed.append(all_polylines[e]) #print(vertices) outside = 0 for entry in removed: all_polylines.remove(entry) #print(len(all_polylines)) for i in range(len(all_polylines)): for point in all_polylines[i].points: vertices.append(point) try: shape = matplotlib.path.Path(vertices, closed=True) except: self.dxf_file_response_pub.publish( String( "dxf file chosen does not contain a closed path, are you sure the file is valid?" )) colors = ['b', 'g', 'r', 'c', 'm', 'y', 'k'] seam_counter = 0 for entity in all_polylines: color = colors[x] sine_vals = [] motion_points = [] for i in range(len(entity.points) - 1): plt.plot([entity.points[i][0], entity.points[i + 1][0]], [entity.points[i][1], entity.points[i + 1][1]], color) #spamwriter.writerow("%f %f"%(entity.points[i][0],entity.points[i][1])) if (i == 0): magnitude = self.distance_calculator( entity.points[i], entity.points[i + 1]) x_vals = [entity.points[i][0], entity.points[i + 1][0]] y_vals = [entity.points[i][1], entity.points[i + 1][1]] else: magnitude = self.distance_calculator( entity.points[i - 1], entity.points[i + 1]) x_vals = [ entity.points[i - 1][0], entity.points[i][0], entity.points[i + 1][0] ] y_vals = [ entity.points[i - 1][1], entity.points[i][1], entity.points[i + 1][1] ] #bestfitlineslope, offset=np.polyfit(x_vals,y_vals,1) #m=(entity.points[i+1][1]-entity.points[i][1])/(entity.points[i+1][0]-entity.points[i][0]) #print(entity.points[i+1][0]-entity.points[i][0]) #print(magnitude/bestfitlineslope) bestfitlineslope, offset = np.polyfit(x_vals, y_vals, 1) #print("best fit line:") #print(bestfitlineslope) #print(shape.contains_point([entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1))])) #plt.arrow(entity.points[i][0],entity.points[i][1],bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),1/(math.sqrt(bestfitlineslope**2+1))) #plt.plot(entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1)),'bo') if (shape.contains_point([ entity.points[i][0] + bestfitlineslope / (-math.sqrt(bestfitlineslope**2 + 1)), entity.points[i][1] + 1 / (math.sqrt(bestfitlineslope**2 + 1)) ])): outside += 1 value = bestfitlineslope / ( math.sqrt(bestfitlineslope**2 + 1)) / (-1 / (math.sqrt(bestfitlineslope**2 + 1))) plt.arrow( entity.points[i][0], entity.points[i][1], bestfitlineslope / (math.sqrt(bestfitlineslope**2 + 1)), -1 / (math.sqrt(bestfitlineslope**2 + 1))) sine_vals.append( math.atan2( -1 / (math.sqrt(bestfitlineslope**2 + 1)), bestfitlineslope / (math.sqrt(bestfitlineslope**2 + 1)))) else: value = bestfitlineslope / ( -math.sqrt(bestfitlineslope**2 + 1)) / ( 1 / (math.sqrt(bestfitlineslope**2 + 1))) #print([entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1))]) plt.arrow( entity.points[i][0], entity.points[i][1], bestfitlineslope / (-math.sqrt(bestfitlineslope**2 + 1)), 1 / (math.sqrt(bestfitlineslope**2 + 1))) sine_vals.append( math.atan2( 1 / (math.sqrt(bestfitlineslope**2 + 1)), bestfitlineslope / (-math.sqrt(bestfitlineslope**2 + 1)))) #plt.plot(entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1)),'bo') #plt.arrow(entity.points[i][0],entity.points[i][1],-(entity.points[i+1][1]-entity.points[i][1])/magnitude,(entity.points[i+1][0]-entity.points[i][0])/magnitude) #print(distance_calculator(entity.points[i],[entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1))])) #print(shape.contains_point([entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1))])) #print(end) #plt.plot([entity.points[i][0],entity.points[i][1]],[entity.points[i][0]-end,entity.points[i][1]+end]) motion_points.append(entity.points[i]) x_vals = [entity.points[-2][0], entity.points[-1][0]] y_vals = [entity.points[-2][1], entity.points[-1][1]] bestfitlineslope, offset = np.polyfit(x_vals, y_vals, 1) if (shape.contains_point([ entity.points[i][0] + bestfitlineslope / (-math.sqrt(bestfitlineslope**2 + 1)), entity.points[i][1] + 1 / (math.sqrt(bestfitlineslope**2 + 1)) ])): outside += 1 value = bestfitlineslope / ( math.sqrt(bestfitlineslope**2 + 1)) / (-1 / (math.sqrt(bestfitlineslope**2 + 1))) plt.arrow( entity.points[-1][0], entity.points[-1][1], bestfitlineslope / (math.sqrt(bestfitlineslope**2 + 1)), -1 / (math.sqrt(bestfitlineslope**2 + 1))) sine_vals.append( math.atan2( -1 / (math.sqrt(bestfitlineslope**2 + 1)), bestfitlineslope / (math.sqrt(bestfitlineslope**2 + 1)))) else: plt.arrow( entity.points[-1][0], entity.points[-1][1], bestfitlineslope / (-math.sqrt(bestfitlineslope**2 + 1)), 1 / (math.sqrt(bestfitlineslope**2 + 1))) sine_vals.append( math.atan2( 1 / (math.sqrt(bestfitlineslope**2 + 1)), bestfitlineslope / (-math.sqrt(bestfitlineslope**2 + 1)))) x += 1 #sine_vals.append(math.asin(value)) motion_points.append(entity.points[-1]) #print(sine_vals) if (seam_counter == 0): self.seam_point_dict["bottom_seam"] = [ motion_points, sine_vals ] elif (seam_counter == 1): self.seam_point_dict["in_seam"] = [motion_points, sine_vals] elif (seam_counter == 2): self.seam_point_dict["j_seam"] = [motion_points, sine_vals] elif (seam_counter == 3): self.seam_point_dict["top_seam"] = [motion_points, sine_vals] elif (seam_counter == 4): self.seam_point_dict["out_seam"] = [motion_points, sine_vals] else: self.dxf_file_response_pub.publish( String( "dxf file contained too many seams, check to see if dxf file is valid" )) #plan, fraction, points=self.plan_robot_motion(motion_points,sine_vals) #if(fraction<1.0): # self.execute_consecutive_motions(points) #else: #self.display_robot_trajectory(plan) #self.execute_planned_trajectory(plan) if (x + 1 > len(colors)): x = 0 seam_counter += 1 #spamwriter.writerow("") #print("end") #print(self.seam_point_dict) self.dxf_file_response_pub.publish( String("Successfully parsed dxf file")) #print(outside) #plt.show() """if(all_lines[0].start[0]>all_lines[1].start[0]):
class ManipulatorSimpleModel: def __init__(self, urdf_file_name, base_link="base_link", ee_name=_EE_NAME): ''' Simple model of manipulator kinematics and controls Assume following state and action vectors urdf_file_name - model file to load ''' # Load KDL tree urdf_file = file(urdf_file_name, 'r') self.robot = Robot.from_xml_string(urdf_file.read()) urdf_file.close() self.tree = kdl_tree_from_urdf_model(self.robot) task_space_ik_weights = np.diag([1.0, 1.0, 1.0, 0.0, 0.0, 0.0]).tolist() #self.base_link = self.robot.get_root() self.base_link = base_link self.joint_chains = [] self.chain = KDLKinematics(self.robot, self.base_link, ee_name) ''' for l_name in _LINK_NAMES: jc=KDLKinematics(self.robot,self.base_link,l_name) self.joint_chains.append(jc) ''' def FK_joint(self, joint_angles, j_index): ''' Method to return task coordinates between base link and any joint joint_angles must contain only 0:j_index joints ''' fi_x = self.joint_chains[j_index].forward(joint_angles) return fi_x def Jacobian_joint(self, joint_angles, j_index): ji_x = self.joint_chains[j_index].jacobian(joint_angles) return ji_x def FK(self, joint_angles): ''' Method to convert joint positions to task coordinates ''' fi_x = self.chain.forward(joint_angles) return fi_x def Jacobian(self, joint_angles): ji_x = self.chain.jacobian(joint_angles) return ji_x #TODO: CHECK if IK works def IK(self, fingers_desired, finger=None): ''' Get inverse kinematics for desired finger poses of all fingers fingers_desired - a list of desired finger tip poses in order of finger_chains[] returns - a list of lists of pyhton arrays of finger joint configurations TODO: Allow for single finger computation (named fingers) ''' q_desired_fingers = [] if finger is not None: # TODO: solve for a single finger... return None for i, f_d in enumerate(fingers_desired): q_desired_fingers.append(self.finger_chains[i].inverse_wdls(f_d)) # q_desired_fingers.append(self.finger_chains[i].inverse(f_d)) return q_desired_fingers
class SimplePlanning: skip_tol = 1e-6 # How you set these options will determine how we do planning: # what inverse kinematics are used for queries, etc. Most of these are # meant to be directly inherited/provided by the CoSTAR Arm class. def __init__(self, robot, base_link, end_link, group, move_group_ns="move_group", planning_scene_topic="planning_scene", robot_ns="", verbose=False, kdl_kin=None, closed_form_IK_solver=None, joint_names=[]): self.robot = robot self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if kdl_kin is None: self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) else: self.kdl_kin = kdl_kin self.base_link = base_link self.joint_names = joint_names self.end_link = end_link self.group = group self.robot_ns = robot_ns self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction) self.acceleration_magnification = 1 rospy.wait_for_service('compute_cartesian_path') self.cartesian_path_plan = rospy.ServiceProxy('compute_cartesian_path', GetCartesianPath) self.verbose = verbose self.closed_form_IK_solver = closed_form_IK_solver # Basic ik() function call. # It handles calls to KDL inverse kinematics or to the closed form ik # solver that you provided. def ik(self, T, q0, dist=0.5): q = None if self.closed_form_IK_solver is not None: #T = pm.toMatrix(F) q = self.closed_form_IK_solver.findClosestIK(T, q0) else: q = self.kdl_kin.inverse(T, q0) return q # Compute parameters for a nice trapezoidal motion. This will let us create # movements with the basic move() operation that actually look pretty nice. def calculateAccelerationProfileParameters( self, dq_to_target, # joint space offset to target base_steps, # number of trajectory points to create steps_per_meter, # number of extra traj pts to make steps_per_radians, # as above delta_translation, time_multiplier, percent_acc): # We compute a number of steps to take along our trapezoidal trajectory # curve. This gives us a nice, relatively dense trajectory that we can # introspect on later -- we can use it to compute cost functions, to # detect collisions, etc. delta_q_norm = np.linalg.norm(dq_to_target) steps = base_steps + delta_translation * steps_per_meter + delta_q_norm \ * steps_per_radians # Number of steps must be an int. steps = int(np.round(steps)) # This is the time needed for constant velocity at 100% to reach the goal. t_v_constant = delta_translation + delta_q_norm ts = (t_v_constant / steps) * time_multiplier # the max constant joint velocity dq_max_target = np.max(np.absolute(dq_to_target)) v_max = dq_max_target / t_v_constant v_setting_max = v_max / time_multiplier acceleration = v_max * percent_acc # j_acceleration = np.array(dq_target) / t_v_constant * percent_acc t_v_setting_max = v_setting_max / acceleration # Compute the number of trajectory points we want to make before we will # get up to max speed. steps_to_max_speed = 0.5 * acceleration * t_v_setting_max **2 \ / (dq_max_target / steps) if steps_to_max_speed * 2 > steps: rospy.logwarn("Cannot reach the maximum velocity setting, steps " "required %.1f > total number of steps %d" % (steps_to_max_speed * 2, steps)) t_v_setting_max = np.sqrt(0.5 * dq_max_target / acceleration) steps_to_max_speed = (0.5 * steps) v_setting_max = t_v_setting_max * acceleration rospy.loginfo("Acceleration number of steps is set to %.1f and time " "elapsed to reach max velocity is %.3fs" % (steps_to_max_speed, t_v_setting_max)) const_velocity_max_step = np.max([steps - 2 * steps_to_max_speed, 0]) return steps, ts, t_v_setting_max, steps_to_max_speed, const_velocity_max_step # This is where we compute what time we want for each trajectory point. def calculateTimeOfTrajectoryStep(self, step_index, steps_to_max_speed, const_velocity_max_step, t_v_const_step, t_to_reach_v_setting_max): acceleration_step = np.min([step_index, steps_to_max_speed]) deceleration_step = np.min([ np.max( [step_index - const_velocity_max_step - steps_to_max_speed, 0]), steps_to_max_speed ]) const_velocity_step = np.min([ np.max([0, step_index - steps_to_max_speed]), const_velocity_max_step ]) acceleration_time = 0 deceleration_time = 0 if steps_to_max_speed > 0.0001: acceleration_time = np.sqrt( acceleration_step / steps_to_max_speed) * t_to_reach_v_setting_max deceleration_time = t_to_reach_v_setting_max - np.sqrt( (steps_to_max_speed - deceleration_step) / steps_to_max_speed) * t_to_reach_v_setting_max const_vel_time = const_velocity_step * t_v_const_step return acceleration_time + const_vel_time + deceleration_time # Compute a nice joint trajectory. This is useful for checking collisions, # and ensuring that we have nice, well defined behavior. def getJointMove(self, q_goal, q0, base_steps=1000, steps_per_meter=1000, steps_per_radians=4, time_multiplier=1, percent_acc=1, use_joint_move=False, table_frame=None): if q0 is None: rospy.logerr("Invalid initial joint position in getJointMove") return JointTrajectory() elif np.all(np.isclose(q0, q_goal, atol=0.0001)): rospy.logwarn("Robot is already in the goal position.") return JointTrajectory() if np.any(np.greater(np.absolute(q_goal[:2] - np.array(q0[:2])), np.pi/2)) \ or np.absolute(q_goal[3] - q0[3]) > np.pi: # TODO: these thresholds should not be set manually here. rospy.logerr("Dangerous IK solution, abort getJointMove") return JointTrajectory() delta_q = np.array(q_goal) - np.array(q0) # steps = base_steps + int(np.sum(np.absolute(delta_q)) * steps_per_radians) steps, t_v_const_step, t_v_setting_max, steps_to_max_speed, const_velocity_max_step = self.calculateAccelerationProfileParameters( delta_q, base_steps, 0, steps_per_radians, 0, time_multiplier, self.acceleration_magnification * percent_acc) traj = JointTrajectory() traj.points.append( JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0))) # compute IK for i in range(1, steps + 1): xyz = None rpy = None q = None q = np.array(q0) + (float(i) / steps) * delta_q q = q.tolist() if self.verbose: print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q)) if q is not None: dq_i = np.array(q) - np.array(traj.points[i - 1].positions) if np.sum(dq_i) < 0.0001: rospy.logwarn( "Joint trajectory point %d is repeating previous trajectory point. " % i) # continue total_time = total_time = self.calculateTimeOfTrajectoryStep( i, steps_to_max_speed, const_velocity_max_step, t_v_const_step, t_v_setting_max) traj.points[i - 1].velocities = dq_i / ( total_time - traj.points[i - 1].time_from_start.to_sec()) pt = JointTrajectoryPoint(positions=q, velocities=[0] * len(q), accelerations=[0] * len(q)) pt.time_from_start = rospy.Duration(total_time) traj.points.append(pt) else: rospy.logwarn( "No IK solution on one of the trajectory point to cartesian move target" ) if len(traj.points) < base_steps: print rospy.logerr("Planning failure with " \ + str(len(traj.points)) \ + " / " + str(base_steps) \ + " points.") return JointTrajectory() traj.joint_names = self.joint_names return traj # Compute a simple trajectory. def getCartesianMove(self, frame, q0, base_steps=1000, steps_per_meter=1000, steps_per_radians=4, time_multiplier=1, percent_acc=1, use_joint_move=False, table_frame=None): if table_frame is not None: if frame.p[2] < table_frame[0][2]: rospy.logerr( "Ignoring move to waypoint due to relative z: %f < %f" % (frame.p[2], table_frame[0][2])) return JointTrajectory() if q0 is None: rospy.logerr("Invalid initial joint position in getCartesianMove") return JointTrajectory() # interpolate between start and goal pose = pm.fromMatrix(self.kdl_kin.forward(q0)) cur_rpy = np.array(pose.M.GetRPY()) cur_xyz = np.array(pose.p) goal_rpy = np.array(frame.M.GetRPY()) goal_xyz = np.array(frame.p) delta_rpy = np.linalg.norm(goal_rpy - cur_rpy) delta_translation = (pose.p - frame.p).Norm() if delta_rpy < 0.001 and delta_translation < 0.001: rospy.logwarn("Robot is already in the goal position.") return JointTrajectory(points=[ JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0), time_from_start=rospy.Duration(0.0)) ], joint_names=self.joint_names) q_target = self.ik(pm.toMatrix(frame), q0) if q_target is None: rospy.logerr("No IK solution on cartesian move target") return JointTrajectory() else: if np.any( np.greater( np.absolute(q_target[:2] - np.array(q0[:2])), np.pi/2)) \ or np.absolute(q_target[3] - q0[3]) > np.pi: rospy.logerr("Dangerous IK solution, abort getCartesianMove") return JointTrajectory() dq_target = q_target - np.array(q0) if np.sum(np.absolute(dq_target)) < 0.0001: rospy.logwarn("Robot is already in the goal position.") return JointTrajectory(points=[ JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0), time_from_start=rospy.Duration(0.0)) ], joint_names=self.joint_names) steps, t_v_const_step, t_v_setting_max, steps_to_max_speed, const_velocity_max_step = self.calculateAccelerationProfileParameters( dq_target, base_steps, steps_per_meter, steps_per_radians, delta_translation, time_multiplier, self.acceleration_magnification * percent_acc) traj = JointTrajectory() traj.points.append( JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0))) # Compute a smooth trajectory. for i in range(1, steps + 1): xyz = None rpy = None q = None if not use_joint_move: xyz = cur_xyz + ((float(i) / steps) * (goal_xyz - cur_xyz)) rpy = cur_rpy + ((float(i) / steps) * (goal_rpy - cur_rpy)) # Create transform for goal frame frame = pm.toMatrix( kdl.Frame(kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]), kdl.Vector(xyz[0], xyz[1], xyz[2]))) # Use current inverse kinematics solver with current position q = self.ik(frame, q0) else: q = np.array(q0) + (float(i) / steps) * dq_target q = q.tolist() #q = self.kdl_kin.inverse(frame,q0) if self.verbose: print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q)) if q is not None: total_time = self.calculateTimeOfTrajectoryStep( i, steps_to_max_speed, const_velocity_max_step, t_v_const_step, t_v_setting_max) # Compute the distance to the last point for each joint. We use this to compute our joint velocities. dq_i = np.array(q) - np.array(traj.points[-1].positions) if np.sum(np.abs(dq_i)) < self.skip_tol: rospy.logwarn( "Joint trajectory point %d is repeating previous trajectory point. " % i) continue traj.points[i - 1].velocities = (dq_i) / ( total_time - traj.points[i - 1].time_from_start.to_sec()) pt = JointTrajectoryPoint(positions=q, velocities=[0] * len(q), accelerations=[0] * len(q)) pt.time_from_start = rospy.Duration(total_time) # pt.time_from_start = rospy.Duration(i * ts) traj.points.append(pt) else: rospy.logwarn( "No IK solution on one of the trajectory point to cartesian move target" ) if len(traj.points) < base_steps: print rospy.logerr("Planning failure with " \ + str(len(traj.points)) \ + " / " + str(base_steps) \ + " points.") return JointTrajectory() traj.joint_names = self.joint_names return traj def getGoalConstraints(self, frame=None, q=None, q_goal=None, timeout=2.0, mode=ModeJoints): if frame == None and q_goal == None: raise RuntimeError( 'Must provide either a goal frame or joint state!') return (None, None) if q is None: raise RuntimeError('Must provide starting position!') return (None, None) if len(self.robot_ns) > 0: srv = rospy.ServiceProxy(self.robot_ns + "/compute_ik", moveit_msgs.srv.GetPositionIK) else: srv = rospy.ServiceProxy("compute_ik", moveit_msgs.srv.GetPositionIK) goal = Constraints() if frame is not None: joints = self.ik(pm.toMatrix(frame), q) elif q_goal is not None: joints = q_goal if len(joints) is not len(self.joint_names): rospy.logerr( "Invalid goal position. Number of joints in goal is not the same as robot's dof" ) return (None, None) if mode == ModeJoints or q_goal is not None: for i in range(0, len(self.joint_names)): joint = JointConstraint() joint.joint_name = self.joint_names[i] joint.position = joints[i] joint.tolerance_below = 1e-6 joint.tolerance_above = 1e-6 joint.weight = 1.0 goal.joint_constraints.append(joint) else: print 'Setting cartesian constraint' # TODO: Try to fix this again. Something is wrong cartesian_costraint = PositionConstraint() cartesian_costraint.header.frame_id = 'base_link' cartesian_costraint.link_name = self.joint_names[-1] # cartesian_costraint.target_point_offset = frame.p bounding_volume = BoundingVolume() sphere_bounding = SolidPrimitive() sphere_bounding.type = sphere_bounding.SPHERE # constrain position with sphere 1 mm around target sphere_bounding.dimensions.append(0.5) bounding_volume.primitives.append(sphere_bounding) sphere_pose = Pose() sphere_pose.position = frame.p sphere_pose.orientation.w = 1.0 bounding_volume.primitive_poses.append(sphere_pose) cartesian_costraint.constraint_region = bounding_volume cartesian_costraint.weight = 1.0 goal.position_constraints.append(cartesian_costraint) orientation_costraint = OrientationConstraint() orientation_costraint.header.frame_id = 'base_link' orientation_costraint.link_name = self.joint_names[-1] orientation_costraint.orientation = frame.M.GetQuaternion() orientation_costraint.absolute_x_axis_tolerance = 0.1 orientation_costraint.absolute_y_axis_tolerance = 0.1 orientation_costraint.absolute_z_axis_tolerance = 0.1 orientation_costraint.weight = 1.0 goal.orientation_constraints.append(orientation_costraint) print 'Done' return (None, goal) def updateAllowedCollisions(self, obj, allowed): self.planning_scene_publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) rospy.wait_for_service('get_planning_scene', 10.0) get_planning_scene = rospy.ServiceProxy('get_planning_scene', GetPlanningScene) request = PlanningSceneComponents( components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = get_planning_scene(request) acm = response.scene.allowed_collision_matrix if not obj in acm.default_entry_names: # add button to allowed collision matrix acm.default_entry_names += [obj] acm.default_entry_values += [allowed] else: idx = acm.default_entry_names.index(obj) acm.default_entry_values[idx] = allowed #if obj in acm.entry_names: # idx = acm.entry_names.idx # for entry in acm.entry_values: # entry.enabled[idx] = allowed # for i in xrange(len(acm.entry_names)): # acm.entry_values[idx].enabled[i]=allowed planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) self.planning_scene_publisher.publish(planning_scene_diff) def getPlan(self, frame=None, q=None, q_goal=None, obj=None, compute_ik=True): planning_options = PlanningOptions() planning_options.plan_only = True planning_options.replan = False planning_options.replan_attempts = 0 planning_options.replan_delay = 0.1 planning_options.planning_scene_diff.is_diff = True planning_options.planning_scene_diff.robot_state.is_diff = True if frame is None and q_goal is None: raise RuntimeError( 'Must provide either a goal frame or joint state!') if q is None: raise RuntimeError('Must provide starting position!') elif len(q) is not len(self.joint_names): rospy.logerr( "Invalid number of joints in getPlan starting position setting" ) return (-31, None) if obj is not None: self.updateAllowedCollisions(obj, True) motion_req = MotionPlanRequest() motion_req.start_state.joint_state.position = q motion_req.start_state.joint_state.name = self.joint_names motion_req.workspace_parameters.header.frame_id = self.base_link motion_req.workspace_parameters.max_corner.x = 1.0 motion_req.workspace_parameters.max_corner.y = 1.0 motion_req.workspace_parameters.max_corner.z = 1.0 motion_req.workspace_parameters.min_corner.x = -1.0 motion_req.workspace_parameters.min_corner.y = -1.0 motion_req.workspace_parameters.min_corner.z = -1.0 # create the goal constraints # TODO: change this to use cart goal(s) # - frame: take a list of frames # - returns: goal contraints constrain_mode = ModeJoints if compute_ik: (ik_resp, goal) = self.getGoalConstraints(frame=frame, q=q, q_goal=q_goal, mode=constrain_mode) motion_req.goal_constraints.append(goal) motion_req.group_name = self.group motion_req.num_planning_attempts = 10 motion_req.allowed_planning_time = 4.0 motion_req.planner_id = "RRTConnectkConfigDefault" if goal is None: print 'Error: goal is None' return (-31, None) elif constrain_mode == ModeJoints and motion_req is not None and len( motion_req.goal_constraints[0].joint_constraints) == 0: print 'Error: joint constraints length is 0' return (-31, None) elif ((not ik_resp is None and ik_resp.error_code.val < 0) or (not ik_resp is None and ik_resp.error_code.val < 0)): print 'Error: ik resp failure' return (-31, None) goal = MoveGroupGoal() goal.planning_options = planning_options goal.request = motion_req rospy.logwarn("Sending request...") self.client.send_goal(goal) self.client.wait_for_result() res = self.client.get_result() if res is not None: rospy.logwarn("Done: " + str(res.error_code.val)) if obj is not None: self.updateAllowedCollisions(obj, False) return (res.error_code.val, res) else: rospy.logerr("Planning response is None") return (-31, None) def getPlanWaypoints(self, waypoints_in_kdl_frame, q, obj=None): cartesian_path_req = GetCartesianPathRequest() cartesian_path_req.header.frame_id = self.base_link cartesian_path_req.start_state = RobotState() cartesian_path_req.start_state.joint_state.name = self.joint_names if type(q) is list: cartesian_path_req.start_state.joint_state.position = q else: cartesian_path_req.start_state.joint_state.position = q.tolist() cartesian_path_req.group_name = self.group cartesian_path_req.link_name = self.joint_names[-1] cartesian_path_req.avoid_collisions = False cartesian_path_req.max_step = 50 cartesian_path_req.jump_threshold = 0 # cartesian_path_req.path_constraints = Constraints() if obj is not None: self.updateAllowedCollisions(obj, True) cartesian_path_req.waypoints = list() for T in waypoints_in_kdl_frame: cartesian_path_req.waypoints.append(pm.toMsg(T)) res = self.cartesian_path_plan.call(cartesian_path_req) if obj is not None: self.updateAllowedCollisions(obj, False) return (res.error_code.val, res)
def build_tree(iter,treeA, treeB ,edgesA, edgesB, plot, kdl_kin): i = 0 while i < iter: # print(i) jointsA = np.random.rand(1,7)[0]*[2.5, 2.5, 4, 1.5, 4, 2.5, 4] - [1.25, 1.25, 2.0, .75, 2.0, .75, 2.0] velA = np.random.rand(1,7)[0]*[3.0,3.0,3.0,3.0,6.0,3.0,6.0] - [1.5,1.5,1.5,1.5,2,1.5,2] node = nearest_neighbor(jointsA, velA, treeA, i) treeA = insert_vertex(node,treeA,jointsA,velA,i,1) edgesA = insert_edge(edgesA, node, i) jointsB = np.random.rand(1,7)[0]*[2.5, 2.5, 4, 1.5, 4, 2.5, 4] - [1.25, 1.25, 2.0, .75, 2.0, .75, 2.0] velB = np.random.rand(1,7)[0]*[3.0,3.0,3.0,3.0,6.0,3.0,6.0] - [1.5,1.5,1.5,1.5,2,1.5,2] node = nearest_neighbor(jointsB, velB, treeB, i) treeB = insert_vertex(node,treeB,jointsB,velB,i,1) edgesB = insert_edge(edgesB, node, i) result = connect_trees(treeA, treeB, i, kdl_kin) # print(result[0],i) if (result[0]): print "iterated up to: " print i break i = i + 1 iterations = i if (plot): # plt.close('all') # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.scatter(treeA[0,0],treeA[0,1],treeA[0,2],'r') # ax.plot(treeA[0:i+2,0],treeA[0:i+2,1],treeA[0:i+2,2],'r.') # ax.scatter(treeB[0,0],treeB[0,1],treeB[0,2],'b') # ax.plot(treeB[0:i+2,0],treeB[0:i+2,1],treeB[0:i+2,2],'b.') # plt.show(block=False) robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') pos3A = np.empty((iterations,3)) for i in range(iterations): pos3A[i,:]=kdl_kin.forward(treeA[i,:7])[0:3,3].transpose() pos3B = np.empty((iterations,3)) for i in range(iterations): pos3B[i,:]=kdl_kin.forward(treeB[i,:7])[0:3,3].transpose() fig = plt.figure(1) plt.hold(False) # ax = fig.gca(projection='3d') plt.plot(pos3A[:,1],pos3A[:,2],'r.') plt.hold(True) plt.plot(pos3A[0,1],pos3A[0,2],'r.',markersize=15) plt.plot(pos3B[:,1],pos3B[:,2],'b.') plt.plot(pos3B[0,1],pos3B[0,2],'b.',markersize=15) plt.xlabel('EE y-coordinate') plt.ylabel('EE z-coordinate') plt.title('RRT in end-effector space') # plt.show(block=False) if (result[0]): indA = result[1]; indB = result[2]; # if (plot): # ax.scatter([treeA[indA,0], treeB[indB,0]],[treeA[indA,1], treeB[indB,1]],[treeA[indA,2], treeB[indB,2]],'g') else: indA = 0 indB = 0 if (plot): # plt.xlim([-1.5, 1.5]) # plt.ylim([-1.5, 1.5]) for k in range(iterations+1): edge1 = kdl_kin.forward(treeA[edgesA[k,0],:7])[0:3,3].transpose() edge2 = kdl_kin.forward(treeA[edgesA[k,1],:7])[0:3,3].transpose() plt.plot([edge1[0,1],edge2[0,1]],[edge1[0,2],edge2[0,2]],'r',linewidth=3) edge1 = kdl_kin.forward(treeB[edgesB[k,0],:7])[0:3,3].transpose() edge2 = kdl_kin.forward(treeB[edgesB[k,1],:7])[0:3,3].transpose() plt.plot([edge1[0,1],edge2[0,1]],[edge1[0,2],edge2[0,2]],'b',linewidth=3) # plt.show(block=False) # raw_input('Enter...') return treeA[0:iterations+2,:], treeB[0:iterations+2,:], edgesA[0:iterations+1,:], edgesB[0:iterations+1,:], indA, indB
class KinematicsTest(object): """Test kinematics using KDL""" def __init__(self): """Read robot describtion""" self.robot_ = URDF.from_parameter_server() self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector') self.cur_pose_ = None self.cur_jnt_ = None # Creates the SimpleActionClient, passing the type of the action self.client_ = actionlib.SimpleActionClient( '/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction) self.client_.wait_for_server() self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal() #time_from_start=rospy.Duration(10) self.goal_.trajectory.joint_names = [ 'j1', 'j2', 'j3', 'j4', 'j5', 'j6' ] #To be reached 1 second after starting along the trajectory trajectory_point = trajectory_msgs.msg.JointTrajectoryPoint() trajectory_point.positions = [0] * 6 trajectory_point.time_from_start = rospy.Duration(0.1) self.goal_.trajectory.points.append(trajectory_point) rospy.Subscriber('joint_states', sensor_msgs.msg.JointState, self.fwd_kinematics_cb) rospy.Subscriber('command', geometry_msgs.msg.Pose, self.inv_kinematics_cb) def fwd_kinematics_cb(self, data): """Compute forward kinematics and print it out :data: TODO :returns: TODO """ self.cur_jnt_ = data.position self.cur_pose_ = self.kdl_kin_.forward(data.position) q = np.array(data.position) new_pose = self.cur_pose_.copy() new_pose[0, 3] += 0.01 #print "cp:", np.ravel(self.cur_pose_[:3,3]) #print "np:", np.ravel(new_pose[:3,3]) #q_ik = self.kdl_kin_.inverse(new_pose, q) #print "Q:", q #print "Q_ik:", q_ik def inv_kinematics_cb(self, data): """calculate the ik given a pose (only xyz right now) :data: TODO :returns: TODO """ # preserve the rotation new_pose = self.cur_pose_.copy() new_pose[0, 3] = data.position.x new_pose[1, 3] = data.position.y new_pose[2, 3] = data.position.z q_ik = self.kdl_kin_.inverse(new_pose, self.cur_jnt_) print "cur:", np.ravel(self.cur_pose_[:3, 3]) print "req:", np.ravel(new_pose[:3, 3]) print "Q:", self.cur_jnt_ print "Q_ik:", q_ik if q_ik is not None: print "Sending goal" self.goal_.trajectory.points[0].positions = q_ik self.client_.send_goal(self.goal_)
def __init__(self, name): self.name = name self.timer = 0 # the names of the base_link and end_link according to the urdf model self.base_link = "iiwa_0_link" self.end_link = "iiwa_adapter" # initialize a node rospy.init_node("mission_node") self.robot = URDF.from_xml_file( "/home/davoud/kuka_ws/src/kuka/kuka/iiwa/iiwa_description/urdf/iiwa.urdf" ) self.tree = kdl_tree_from_urdf_model(self.robot) self.kdl_kin = KDLKinematics(self.robot, self.base_link, self.end_link) # you can use one of these methods: # 1: Jacobian Transpose Method (JTM) # 2: Jacobian Pseudoinverse Method (JPM) # 3: Damped Least Squares Method (DLSM) self.used_method = "DLSM" self.landa = 0.1 # tf broadcaster self.br = tf.TransformBroadcaster() # current position of the end effector self.end_eff_current_pose = np.empty((1, 7)) # transformation matrix between end_effector and camera self.end_cam_T = np.empty((4, 4)) self.set_end_cam_T() # transformation matrix between end_effector and gripper self.end_grip_T = np.empty((4, 4)) self.set_end_grip_T() # transformation matrix between gripper and target pose self.grip_target_T = np.empty((4, 4)) self.set_grip_target_T() # transformation matrix between base and end effector self.base_end_T = np.empty((4, 4)) # transformation matrix between base and target self.base_target_T = np.empty((4, 4)) # needed variables for actionlib self.joint_names = [ "joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7" ] # actionlib commands self.manipulator_client = actionlib.SimpleActionClient( "/iiwa/follow_joint_trajectory", FollowJointTrajectoryAction) self.manipulator_client.wait_for_server() self.trajectory = JointTrajectory() self.trajectory.joint_names = self.joint_names self.trajectory.points.append(JointTrajectoryPoint()) # path of end_effector self.ee_path = MarkerArray() # path of object self.object_path = MarkerArray() # initializ a publisher to publish the marker of detected objet to rviz self.marker_object_in_base_pub = rospy.Publisher( "marker_object_in_base", Marker, queue_size=1) self.marker_object_in_cam_pub = rospy.Publisher("marker_object_in_cam", Marker, queue_size=1) self.marker_eff_path_pub = rospy.Publisher("marker_eff_path", MarkerArray, queue_size=1) self.marker_obj_path_pub = rospy.Publisher("marker_obj_path", MarkerArray, queue_size=1) # initialize the service client for the gripper rospy.wait_for_service("/sdh_action") self.joint_config_client = rospy.ServiceProxy("/sdh_action", SDHAction) # subscribe to "/joint_states" topic which is published by kuka. rospy.Subscriber("/joint_states", JointState, self.kuka_inf_cb) # subscribe to "/sphere_coefs" topic which is published by ball detector using color and distance filters rospy.Subscriber("/sphere_coefs", Float32MultiArray, self.marker_inf_cb, queue_size=1) rospy.spin()
msg = JointState(name=CONFIG['joints'], position=Q) pub.publish(msg) rospy.sleep(0.2) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e: pass if __name__ == '__main__': rospy.init_node('tom_simple_goto') pub = rospy.Publisher('joint_states_cmd', JointState, queue_size=1000) robot = URDF.from_parameter_server() tree = kdl_tree_from_urdf_model(robot) chain = tree.getChain(base_link, end_link) kdl_kin = KDLKinematics(robot, base_link, end_link) """ position: x: 0.648891402264 y: -0.563835865845 z: -0.263676911067 orientation: x: -0.399888401484 y: 0.916082302699 z: -0.0071291983402 w: -0.0288384391252 """ trans, rot = (0.64, -0.56, -0.26), (-0.4, 0.92, -0.01, -0.03)
class YoubotArm: # Constructor. def __init__(self): self.moving_to_new_x_pos = False self.moving_to_new_y_pos = False self.stop_base_movement = False self.max_virtual_x_vel = 0.05 self.max_virtual_y_vel = 0.05 self.commanded_virtual_x_pos = 0.0 self.commanded_virtual_y_pos = 0.0 self.commanded_virtual_x_vel = 0.0 self.commanded_virtual_y_vel = 0.0 self.virtual_x_cmd_time_sec = 0.0 self.virtual_y_cmd_time_sec = 0.0 self.virtual_x_running_time_sec = 0.0 self.virtual_y_running_time_sec = 0.0 youbot_urdf = URDF.from_parameter_server() self.kin_with_virtual = KDLKinematics(youbot_urdf, "virtual", "gripper_palm_link") self.kin_grasp = KDLKinematics(youbot_urdf, "base_link", "gripper_palm_link") # Create a timer that will be used to monitor the velocity of the virtual # joints when we need them to be positioning themselves. self.vel_monitor_timer = rospy.Timer(rospy.Duration(0.1), self.vel_monitor_timer_callback) self.arm_joint_pub = rospy.Publisher("arm_1/arm_controller/position_command",JointPositions,queue_size=10) self.gripper_pub = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions, queue_size=10) self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) # Give the publishers time to get setup before trying to do any actual work. rospy.sleep(2) # Initialize at the candle position. self.publish_arm_joint_positions(armJointPosCandle) rospy.sleep(2.0) # Go through the routine of picking up the block. self.grasp_routine() # A callback function to run on a timer that runs the velocity commands for a specified # amount of time. # # Params: # time_data - the rospy.TimerEvent object passed from the rospy.Timer def vel_monitor_timer_callback(self, time_data): delta_t = 0.0 if time_data.last_real != None: delta_t = time_data.current_real.now().to_sec() - time_data.last_real.now().to_sec() if self.moving_to_new_x_pos or self.moving_to_new_y_pos: # --- Start publishing velocity commands --- # cmd_twist = Twist() if self.moving_to_new_x_pos: if self.commanded_virtual_x_pos > 0: cmd_twist.linear.x = self.max_virtual_x_vel elif self.commanded_virtual_x_pos < 0: cmd_twist.linear.x = -self.max_virtual_x_vel self.virtual_x_running_time_sec += delta_t self.moving_to_new_x_pos = self.virtual_x_running_time_sec <= self.virtual_x_cmd_time_sec if self.moving_to_new_y_pos: if self.commanded_virtual_y_pos > 0: cmd_twist.linear.y = self.max_virtual_y_vel elif self.commanded_virtual_y_pos < 0: cmd_twist.linear.y = -self.max_virtual_y_vel self.virtual_y_running_time_sec += delta_t self.moving_to_new_y_pos = self.virtual_y_running_time_sec <= self.virtual_y_cmd_time_sec self.stop_base_movement = not self.moving_to_new_x_pos and not self.moving_to_new_y_pos self.vel_pub.publish(cmd_twist) elif self.stop_base_movement: # Once we have reached our target, we need to send a zero-velocity twist to stop it. cmd_twist = Twist() self.vel_pub.publish(cmd_twist) self.stop_base_movement = False # Just to be safe, let's reset our timers. if not self.moving_to_new_x_pos and not self.moving_to_new_y_pos: self.virtual_x_running_time_sec = 0.0 self.virtual_y_running_time_sec = 0.0 # Takes in a list of joint angles for joints 1-5 and publishes them for the YouBot to receive. # # Params: # joint_positions - the list of joint positions to publish. Should be for arm joints 1-5. def publish_arm_joint_positions(self, joint_positions): desiredPositions = JointPositions() jointCommands = [] for i in range(5): joint = JointValue() joint.joint_uri = "arm_joint_" + str(i+1) joint.unit = "rad" joint.value = joint_positions[i] jointCommands.append(joint) desiredPositions.positions = jointCommands self.arm_joint_pub.publish(desiredPositions) # Publishes the parameter gripper width to the YouBot to set its position. # # Params: # width - the width value to be applied to both gripper fingers. def publish_gripper_width(self, width): desiredPositions = JointPositions() jointCommands = [] joint = JointValue() joint.joint_uri = "gripper_finger_joint_l" joint.unit = "m" joint.value = width jointCommands.append(joint) joint = JointValue() joint.joint_uri = "gripper_finger_joint_r" joint.unit = "m" joint.value = width jointCommands.append(joint) desiredPositions.positions = jointCommands self.gripper_pub.publish(desiredPositions) # Goes through a routine of predefined poses that starts at a neutral position, moves down # to grasp the grasp an object, and moves back to a neutral position. def grasp_routine(self): # --- Get into position for grasp --- # pose_above_grasp = quat_pos_to_se3(quat_above_grasp, pos_above_grasp) # Try to solve IK for the pickup point. q_ik = self.kin_with_virtual.inverse(pose_above_grasp, jointGuessForGrasp) if q_ik != None: q_ik = np.array(q_ik) # Publish the IK results. arm_above_grasp = [q_ik[2], q_ik[3], q_ik[4], q_ik[5], q_ik[6]] arm_above_grasp = self.limit_arm_joints(arm_above_grasp) self.publish_arm_joint_positions(arm_above_grasp) # Sleep for a while to give the arm time to get there. rospy.sleep(2.0) # --- Open grippers --- # self.publish_gripper_width(gripperWidthOpen) rospy.sleep(2.0) # --- Move down to grasp --- # # Solve IK for the grasp point: pos_at_grasp = pos_above_grasp.copy() pos_at_grasp[0:2] -= q_ik[0:2] pos_at_grasp[2] -= 0.1 # move down 10 cm pose_grasp = quat_pos_to_se3(quat_above_grasp, pos_at_grasp) q_grasp = dls_ik(self.kin_grasp, pose_grasp, q_ik[2:]) q_grasp = self.limit_arm_joints(q_grasp) self.publish_arm_joint_positions(q_grasp) rospy.sleep(2.0) # --- Close the grippers --- # self.publish_gripper_width(gripperWidthAtGrasp) rospy.sleep(1.5) # --- Go to the carry position --- # self.publish_arm_joint_positions(armJointPosCandle) rospy.sleep(2.0) # --- Go to dropoff position --- # # Just use the position above grasp self.publish_arm_joint_positions(arm_above_grasp) # Sleep for a while to give the arm time to get there. rospy.sleep(2.0) # --- Open grippers --- # self.publish_gripper_width(gripperWidthOpen) rospy.sleep(2.0) # --- Go to the carry position --- # self.publish_arm_joint_positions(jointCamera) rospy.sleep(2.0) # --- Code for moving the base with virtual joints --- # # Get the position to move the virtual joints to. #self.commanded_virtual_x_pos = q_ik[0] #self.commanded_virtual_y_pos = q_ik[1] # Establish the velocities in the proper directions. #if self.commanded_virtual_x_pos > 0.0: # self.commanded_virtual_x_vel = self.max_virtual_x_vel # self.moving_to_new_x_pos = True # self.virtual_x_running_time_sec = 0.0 #elif self.commanded_virtual_x_pos < 0.0: # self.commanded_virtual_x_vel = self.max_virtual_x_vel # self.moving_to_new_x_pos = True # self.virtual_x_running_time_sec = 0.0 #if self.commanded_virtual_y_pos > 0.0: # self.commanded_virtual_y_vel = self.max_virtual_y_vel # self.moving_to_new_y_pos = True # self.virtual_y_running_time_sec = 0.0 #elif self.commanded_virtual_y_pos < 0.0: # self.commanded_virtual_y_vel = self.max_virtual_y_vel # self.moving_to_new_y_pos = True # self.virtual_y_running_time_sec = 0.0 # Set up the amount of time to run the commands. #self.virtual_x_cmd_time_sec = math.fabs(self.commanded_virtual_x_pos / self.max_virtual_x_vel) #self.virtual_y_cmd_time_sec = math.fabs(self.commanded_virtual_y_pos / self.max_virtual_y_vel) # --- Set the arm --- # #arm_joints = [q_ik[2], q_ik[3], q_ik[4], q_ik[5], q_ik[6]] #arm_joints = self.limit_arm_joints(arm_joints) #self.publish_arm_joint_positions(arm_joints) else: rospy.logwarn("Invalid IK solution!") # Clamps the parameter list of joints for joints 1-5 between their minimum and maximum values. # # Params: # joints - the list of joint angles to limit. Should contain joints 1-5. # # Returns: # The list of limited joint values. def limit_arm_joints(self, joints): for i in range(5): joints[i] = low_high_limit(joints[i], jointMin[i], jointMax[i]) return joints
class KinematicsTest(object): """Test kinematics using KDL""" def __init__(self): """Read robot describtion""" self.robot_ = URDF.from_parameter_server() self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector') self.cur_pose_ = None self.cur_jnt_ = None # Creates the SimpleActionClient, passing the type of the action self.client_ = actionlib.SimpleActionClient('/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction) self.client_.wait_for_server() self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal() #time_from_start=rospy.Duration(10) self.goal_.trajectory.joint_names=['j1','j2','j3','j4','j5','j6'] #To be reached 1 second after starting along the trajectory trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint() trajectory_point.positions=[0]*6 trajectory_point.time_from_start=rospy.Duration(0.1) self.goal_.trajectory.points.append(trajectory_point) rospy.Subscriber('joint_states', sensor_msgs.msg.JointState, self.fwd_kinematics_cb) rospy.Subscriber('command', geometry_msgs.msg.Pose, self.inv_kinematics_cb) def fwd_kinematics_cb(self, data): """Compute forward kinematics and print it out :data: TODO :returns: TODO """ self.cur_jnt_ = data.position self.cur_pose_ = self.kdl_kin_.forward(data.position) q = np.array(data.position) new_pose = self.cur_pose_.copy() new_pose[0,3] += 0.01 #print "cp:", np.ravel(self.cur_pose_[:3,3]) #print "np:", np.ravel(new_pose[:3,3]) #q_ik = self.kdl_kin_.inverse(new_pose, q) #print "Q:", q #print "Q_ik:", q_ik def inv_kinematics_cb(self, data): """calculate the ik given a pose (only xyz right now) :data: TODO :returns: TODO """ # preserve the rotation new_pose = self.cur_pose_.copy() new_pose[0,3] = data.position.x new_pose[1,3] = data.position.y new_pose[2,3] = data.position.z q_ik = self.kdl_kin_.inverse(new_pose, self.cur_jnt_) print "cur:", np.ravel(self.cur_pose_[:3,3]) print "req:", np.ravel(new_pose[:3,3]) print "Q:", self.cur_jnt_ print "Q_ik:", q_ik if q_ik is not None: print "Sending goal" self.goal_.trajectory.points[0].positions = q_ik self.client_.send_goal(self.goal_)
def __init__(self): rospy.init_node('ur_simulation',anonymous=True) rospy.logwarn('SIMPLE_UR SIMULATION DRIVER LOADING') # TF self.broadcaster_ = tf.TransformBroadcaster() self.listener_ = tf.TransformListener() # SERVICES self.servo_to_pose_service = rospy.Service('simple_ur_msgs/ServoToPose', ServoToPose, self.servo_to_pose_call) self.set_stop_service = rospy.Service('simple_ur_msgs/SetStop', SetStop, self.set_stop_call) self.set_teach_mode_service = rospy.Service('simple_ur_msgs/SetTeachMode', SetTeachMode, self.set_teach_mode_call) self.set_servo_mode_service = rospy.Service('simple_ur_msgs/SetServoMode', SetServoMode, self.set_servo_mode_call) # PUBLISHERS AND SUBSCRIBERS self.driver_status_publisher = rospy.Publisher('/ur_robot/driver_status',String) self.robot_state_publisher = rospy.Publisher('/ur_robot/robot_state',String) self.joint_state_publisher = rospy.Publisher('joint_states',JointState) # self.follow_pose_subscriber = rospy.Subscriber('/ur_robot/follow_goal',PoseStamped,self.follow_goal_cb) # Predicator self.pub_list = rospy.Publisher('/predicator/input', PredicateList) self.pub_valid = rospy.Publisher('/predicator/valid_input', ValidPredicates) rospy.sleep(1) pval = ValidPredicates() pval.pheader.source = rospy.get_name() pval.predicates = ['moving', 'stopped', 'running'] pval.assignments = ['robot'] self.pub_valid.publish(pval) # Rate self.run_rate = rospy.Rate(100) self.run_rate.sleep() ### Set Up Simulated Robot ### self.driver_status = 'IDLE' self.robot_state = 'POWER OFF' robot = URDF.from_parameter_server() self.kdl_kin = KDLKinematics(robot, 'base_link', 'ee_link') # self.q = self.kdl_kin.random_joint_angles() self.q = [-1.5707,-.785,-3.1415+.785,-1.5707-.785,-1.5707,0] # Start Pose? self.start_pose = self.kdl_kin.forward(self.q) self.F_start = tf_c.fromMatrix(self.start_pose) # rospy.logwarn(self.start_pose) # rospy.logwarn(type(self.start_pose)) # pose = self.kdl_kin.forward(q) # joint_positions = self.kdl_kin.inverse(pose, q+0.3) # inverse kinematics # if joint_positions is not None: # pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose # J = self.kdl_kin.jacobian(q) # rospy.logwarn('q:'+str(q)) # rospy.logwarn('joint_positions:'+str(joint_positions)) # rospy.logwarn('pose:'+str(pose)) # if joint_positions is not None: # rospy.logwarn('pose_sol:'+str(pose_sol)) # rospy.logwarn('J:'+str(J)) ### START LOOP ### while not rospy.is_shutdown(): if self.driver_status == 'TEACH': self.update_from_marker() # if self.driver_status == 'SERVO': # self.update_follow() # Publish and Sleep self.publish_status() self.send_command() self.run_rate.sleep() # Finish rospy.logwarn('SIMPLE UR - Simulation Finished')
def linearSpace(plot, T, N, vy, vz, jerk): robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') # Create seed with current position q0 = kdl_kin.random_joint_angles() limb_interface = baxter_interface.limb.Limb('right') current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()] for ind in range(len(q0)): q0[ind] = current_angles[ind] # q_start = np.array([0.2339320701525256, -0.5878981369570848, 0.19903400722813244, 1.8561167533413507, # -0.4908738521233324, -0.97752925707998, -0.49547579448698864]) # q_throw = np.array([0.9265243958827899, -0.7827136970185323, -0.095490304045867, 1.8338740319170121, # -0.03681553890924993, -0.9909515889739773, -0.5840631849873713]) # q_end = np.array([0.9085001216251363, -1.0089758632316308, 0.07401457301547121, 1.8768254939778037, # 0.18599517053110642, -0.8172282647459542, -0.44600491407768406]) q_throw = np.array([ 0.47668453, -0.77274282, 0.93150983, 2.08352941, 0.54149522, -1.26745163, -2.06742261]) q_end = np.array([ 0.75356806, -0.89162633, 0.54648066, 2.08698086, 0.41033986, -1.18423317, -1.15815549]) q_start = np.array([-0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914, -1.16889336, -0.90888362]) X_start = np.asarray(kdl_kin.forward(q_start)) X_throw = np.asarray(kdl_kin.forward(q_throw)) # offset throw position X_throw[0,3] += 0 X_throw[1,3] += 0 X_throw[2,3] += 0 X_end = np.asarray(kdl_kin.forward(q_end)) Vb = np.array([0,0,0,0,vy,vz]) # xStart = X_start # xEnd = X_throw vStart = np.array([0,0,0,0,0,0]) vEnd = Vb aStart = np.array([0,0,0,0,0,0]) aEnd = np.array([0,0,0,0,0,0]) jStart = np.array([0,0,0,0,0,0]) jEnd = np.array([0,0,0,0,0,jerk]) # T = .7 # N = 200*T a = np.array([[1,0,0,0,0,0,0,0],[1,T,T**2,T**3,T**4,T**5,T**6,T**7],[0,1,0,0,0,0,0,0],[0,1,2*T,3*T**2,4*T**3,5*T**4,6*T**5,7*T**6], [0,0,2,0,0,0,0,0],[0,0,2,6*T,12*T**2,20*T**3,30*T**4,42*T**5],[0,0,0,6,0,0,0,0],[0,0,0,6,24*T,60*T**2,120*T**3,210*T**4]]) tSpace = np.linspace(0,T,N); tOffset = T colors = ['r','b','c','y','m','oc','k'] if plot == True: plt.close('all') pLista = np.empty((3,N)) pListb = np.empty((3,N)) vLista = np.empty((3,N)) vListb = np.empty((3,N)) aLista = np.empty((3,N)) aListb = np.empty((3,N)) for i in range(3): b = np.array([X_start[i,3],X_throw[i,3],0,vEnd[3+i],0,0,jStart[i+3],jEnd[i+3]]) coeff = np.linalg.solve(a,b) j1Pa = jointPath(tSpace,coeff) j1Va = jointVelocity(tSpace,coeff) j1Aa = jointAcceleration(tSpace,coeff) b = np.array([X_throw[i,3],X_end[i,3],0,0,0,0,jEnd[i+3],jStart[i+3]]) # b = np.array([X_throw[i,3],X_end[i,3],vEnd[3+i],0,0,0,jEnd[i+3],jStart[i+3]]) coeff = np.linalg.solve(a,b) j1Pb = jointPath(tSpace,coeff) j1Vb = jointVelocity(tSpace,coeff) j1Ab = jointAcceleration(tSpace,coeff) pLista[i,:] = j1Pa pListb[i,:] = j1Pb vLista[i,:] = j1Va vListb[i,:] = j1Vb aLista[i,:] = j1Aa aListb[i,:] = j1Ab vList = np.hstack((vLista,vListb)).transpose() dt = float(T)/(N-1) if plot == True: plt.figure() plt.title('Position (cartesian)') plt.plot(np.hstack((pLista,pListb)).transpose()) plt.figure() plt.title('Velocity (cartesian)') plt.plot(np.hstack((vLista,vListb)).transpose()) # plt.figure() # plt.title('Velocity dt') # vList = np.diff(pLista)/dt # plt.plot(vList.transpose()) plt.figure() plt.title('Acceleration (cartesian)') plt.plot(np.hstack((aLista,aListb)).transpose()) plt.show(block=False) Xlista = np.empty((N,4,4)) Xlistb = np.empty((N,4,4)) Re, pe = TransToRp(X_throw) i=0 for t in tSpace[0:]: pa = pLista[:,i] Ra = Re Xlista[i] = RpToTrans(Ra,pa) pb = pListb[:,i] Rb = Re Xlistb[i] = RpToTrans(Rb,pb) i = i + 1 XlistT = np.vstack((Xlista,Xlistb[1:,:,:])) q0 = kdl_kin.random_joint_angles() current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()] for ind in range(len(q0)): q0[ind] = q_start[ind] thList = np.empty((N*2-1,7)) thList[0] = q0; for i in range(int(2*N)-2): # Solve for joint angles seed = 0 q_ik = kdl_kin.inverse(XlistT[i+1], thList[i]) while q_ik == None: seed += 0.03 q_ik = kdl_kin.inverse(XlistT[i+1], thList[i]+seed) if (seed>1): q_ik = thList[i] break thList[i+1] = q_ik thList = thList[1:,:] # if plot == True: # plt.figure() # plt.plot(thList) # plt.show(block=False) jList = np.empty((thList.shape[0],7)) # Compare jacobian velocities to joint velocities for i in range(thList.shape[0]): jacobian = kdl_kin.jacobian(thList[i]) inv_jac = np.linalg.pinv(jacobian) Vb = np.hstack((np.array([0,0,0]), vList[i])) q_dot_throw = inv_jac.dot(Vb) jList[i,:] = q_dot_throw # plt.figure() # plt.title('Jacobian-based joint velocities') # plt.plot(jList); # plt.show(block = False) vList = np.diff(thList,axis=0)/dt vList = np.vstack((np.array([0,0,0,0,0,0,0]),vList)) vCarlist = np.empty((thList.shape[0],6)) for i in range(thList.shape[0]): jacobian = kdl_kin.jacobian(thList[i]) vCar = jacobian.dot(vList[i]); vCarlist[i,:] = vCar; if (plot==True): plt.figure() plt.title('Jacobian based cartesian velocities') plt.plot(vCarlist[:,0:3]) plt.show(block=False) if (plot==True): plt.figure() plt.title('Joint velocities') plt.plot(vList) plt.show(block=False) return (thList, jList)
class HebiArmController(object): def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link): rospy.loginfo("Creating {} instance".format(self.__class__.__name__)) self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None) if self.openmeta_testbench_manifest_path is not None: self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path) # TestBench Parameters self._params = {} for tb_param in self._testbench_manifest['Parameters']: self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type # TestBench Metrics self._metrics = {} for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO... self._metrics[tb_metric['Name']] = tb_metric['Value'] if hebi_gains is None: hebi_gains = { 'p': [float(self._params['p_gain'])]*3, 'i': [float(self._params['i_gain'])]*3, 'd': [float(self._params['d_gain'])]*3 } hebi_families = [] hebi_names = [] for actuator in hebi_mapping: family, name = actuator.split('/') hebi_families.append(family) hebi_names.append(name) rospy.loginfo(" hebi_group_name: %s", hebi_group_name) rospy.loginfo(" hebi_families: %s", hebi_families) rospy.loginfo(" hebi_names: %s", hebi_names) self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names) # Set PID Gains cmd_msg = CommandMsg() cmd_msg.name = hebi_mapping cmd_msg.settings.name = hebi_mapping cmd_msg.settings.control_strategy = [4, 4, 4] cmd_msg.settings.position_gains.name = hebi_mapping cmd_msg.settings.position_gains.kp = hebi_gains['p'] cmd_msg.settings.position_gains.ki = hebi_gains['i'] cmd_msg.settings.position_gains.kd = hebi_gains['d'] cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me. self.hebi_wrap.send_command_with_acknowledgement(cmd_msg) if base_link is None or end_link is None: robot = Robot.from_xml_string(urdf_str) if not base_link: base_link = robot.get_root() # WARNING: There may be multiple leaf nodes if not end_link: end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0] # pykdl self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link) self._active_joints = self.kdl_fk.get_joint_names() # joint data self.position_fbk = [0.0]*self.hebi_wrap.hebi_count self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count # joint state publisher while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping): rospy.sleep(0.1) self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self.hebi_wrap.add_feedback_callback(self._joint_state_cb) # Set up Waypoint/Trajectory objects self.start_wp = WaypointMsg() self.start_wp.names = self.hebi_wrap.hebi_mapping self.end_wp = copy.deepcopy(self.start_wp) self.goal = TrajectoryGoal() self.goal.waypoints = [self.start_wp, self.end_wp] self._hold_positions = [0.0]*3 self._hold = True self.jointstate = JointState() self.jointstate.name = self.hebi_wrap.hebi_mapping rospy.sleep(1.0) def execute(self, rate, joint_states, durations): max_height = 0.0 while not rospy.is_shutdown(): if len(joint_states) > 0: # start waypoint self.start_wp.positions = self.position_fbk self.start_wp.velocities = [0.0]*self.hebi_wrap.hebi_count self.start_wp.accelerations = [0.0]*self.hebi_wrap.hebi_count # end waypoint self.end_wp.positions = joint_states.pop(0) self.end_wp.velocities = [0.0]*self.hebi_wrap.hebi_count self.end_wp.accelerations = [0.0]*self.hebi_wrap.hebi_count self._hold_positions = self.end_wp.positions # action goal self.goal.times = [0.0, durations.pop()] self._hold = False self.hebi_wrap.trajectory_action_client.send_goal(self.goal) self.hebi_wrap.trajectory_action_client.wait_for_result() self._hold = True # check status if abs(self.position_fbk[0] - self.end_wp.positions[0]) > 0.0872665: print("Failed to achieve objective position.") break else: rospy.sleep(2.0) fk = self.kdl_fk.forward(self.position_fbk) max_height = fk.tolist()[2][3] print("Max Height: {}".format(max_height)) break rate.sleep() if self.openmeta_testbench_manifest_path is not None: steps_completed = 0 if 7 <= len(joint_states): steps_completed = 0 elif 5 <= len(joint_states) < 7: steps_completed = 1 elif 3 <= len(joint_states) < 5: steps_completed = 2 elif len(joint_states) < 3: steps_completed = 3 self._metrics['steps_completed'] = steps_completed self._metrics['max_height'] = max_height self._write_metrics_to_tb_manifest() def _joint_state_cb(self, msg): if not rospy.is_shutdown(): if self._hold: self.jointstate.position = self._hold_positions self.jointstate.velocity = [] self.hebi_wrap.joint_state_publisher.publish(self.jointstate) self.position_fbk = self.hebi_wrap.get_joint_positions() self.velocity_fbk = self.hebi_wrap.get_joint_velocities() self.effort_fbk = self.hebi_wrap.get_joint_efforts() jointstate = JointState() jointstate.header.stamp = rospy.Time.now() jointstate.name = self._active_joints jointstate.position = self.position_fbk jointstate.velocity = self.velocity_fbk jointstate.effort = self.effort_fbk self._joint_state_pub.publish(jointstate) def _write_metrics_to_tb_manifest(self): # Write to testbench_manifest metric for tb_metric in self._testbench_manifest['Metrics']: if tb_metric['Name'] in self._metrics: tb_metric['Value'] = self._metrics[tb_metric['Name']] # Save updated testbench_manifest.json with open(self.openmeta_testbench_manifest_path, 'w') as savefile: json_str = json.dumps(self._testbench_manifest, sort_keys=True, indent=2, separators=(',', ': ')) savefile.write(json_str)
def get_jacabian_from_joint(self, urdfname, jointq, flag): robot = URDF.from_xml_file(urdfname) kdl_kin = KDLKinematics(robot, "base_link", "tool0") J = kdl_kin.jacobian(jointq) pose = kdl_kin.forward(jointq) return J, pose
class CostarArm(object): def __init__(self, base_link, end_link, planning_group, world="/world", listener=None, broadcaster=None, traj_step_t=0.1, max_acc=1, max_vel=1, max_goal_diff=0.02, goal_rotation_weight=0.01, max_q_diff=1e-6, start_js_cb=True, base_steps=10, steps_per_meter=100, closed_form_IK_solver=None, dof=7, perception_ns="/SPServer"): self.world = world self.base_link = base_link self.end_link = end_link self.planning_group = planning_group self.dof = dof self.base_steps = base_steps self.steps_per_meter = steps_per_meter self.MAX_ACC = max_acc self.MAX_VEL = max_vel self.traj_step_t = traj_step_t self.max_goal_diff = max_goal_diff self.max_q_diff = max_q_diff self.goal_rotation_weight = goal_rotation_weight self.at_goal = True self.near_goal = True self.moving = False self.q0 = None #[0] * self.dof self.old_q0 = [0] * self.dof self.cur_stamp = 0 # Set up TF broadcaster if not broadcaster is None: self.broadcaster = broadcaster else: self.broadcaster = tf.TransformBroadcaster() # Set up TF listener and smartmove manager if listener is None: self.listener = tf.TransformListener() else: self.listener = listener # Currently this class does not need a smart waypoint manager. # That will remain in the CoSTAR BT. #self.smartmove_manager = SmartWaypointManager( # listener=self.listener, # broadcaster=self.broadcaster) # TODO: ensure the manager is set up properly # Note that while the waypoint manager is currently a part of CostarArm # If we wanted to set this up for multiple robots it should be treated # as an independent component. self.waypoint_manager = WaypointManager(service=True, broadcaster=self.broadcaster) # Set up services # The CostarArm services let the UI put it into teach mode or anything else self.teach_mode = rospy.Service('/costar/SetTeachMode', SetTeachMode, self.set_teach_mode_call) self.servo_mode = rospy.Service('/costar/SetServoMode', SetServoMode, self.set_servo_mode_call) self.shutdown = rospy.Service('/costar/ShutdownArm', EmptyService, self.shutdown_arm_call) self.servo = rospy.Service('/costar/ServoToPose', ServoToPose, self.servo_to_pose_call) self.plan = rospy.Service('/costar/PlanToPose', ServoToPose, self.plan_to_pose_call) self.smartmove = rospy.Service('/costar/SmartMove', SmartMove, self.smart_move_call) self.js_servo = rospy.Service('/costar/ServoToJointState', ServoToJointState, self.servo_to_joints_call) self.save_frame = rospy.Service('/costar/SaveFrame', SaveFrame, self.save_frame_call) self.save_joints = rospy.Service('/costar/SaveJointPosition', SaveFrame, self.save_joints_call) self.get_waypoints_srv = GetWaypointsService(world=world, service=False, ns=perception_ns) self.driver_status = 'IDLE' # Create publishers. These will send necessary information out about the state of the robot. self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd', JointTrajectoryPoint, queue_size=1000) self.status_publisher = rospy.Publisher('/costar/DriverStatus', String, queue_size=1000) self.display_pub = rospy.Publisher('costar/display_trajectory', DisplayTrajectory, queue_size=1000) self.robot = URDF.from_parameter_server() if start_js_cb: self.js_subscriber = rospy.Subscriber('joint_states', JointState, self.js_cb) self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) # cCreate reference to pyKDL kinematics self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) #self.set_goal(self.q0) self.goal = None self.ee_pose = None self.joint_names = [ joint.name for joint in self.robot.joints[:self.dof] ] self.planner = SimplePlanning( self.robot, base_link, end_link, self.planning_group, kdl_kin=self.kdl_kin, joint_names=self.joint_names, closed_form_IK_solver=closed_form_IK_solver) ''' js_cb listen to robot joint state information ''' def js_cb(self, msg): if len(msg.position) is self.dof: pass self.old_q0 = self.q0 self.q0 = np.array(msg.position) else: rospy.logwarn('Incorrect joint dimensionality') ''' update current position information ''' def update_position(self): if self.q0 is None or self.old_q0 is None: return self.ee_pose = pm.fromMatrix(self.kdl_kin.forward(self.q0)) if self.goal is not None: #goal_diff = np.abs(self.goal - self.q0).sum() / self.q0.shape[0] cart_diff = 0 #(self.ee_pose.p - self.goal.p).Norm() rot_diff = 0 #self.goal_rotation_weight * (pm.Vector(*self.ee_pose.M.GetRPY()) - pm.Vector(*self.goal.M.GetRPY())).Norm() goal_diff = cart_diff + rot_diff if goal_diff < self.max_goal_diff: self.at_goal = True if goal_diff < 10 * self.max_goal_diff: self.near_goal = True q_diff = np.abs(self.old_q0 - self.q0).sum() if q_diff < self.max_q_diff: self.moving = False else: self.moving = True def check_req_speed_params(self, req): if req.accel > self.MAX_ACC: acceleration = self.MAX_ACC else: acceleration = req.accel if req.vel > self.MAX_VEL: velocity = self.MAX_VEL else: velocity = req.vel return (acceleration, velocity) ''' Save the current end effector pose as a frame that we can return to ''' def save_frame_call(self, req): rospy.logwarn( 'Save frame does not check to see if your frame already exists!') print self.ee_pose self.waypoint_manager.save_frame(self.ee_pose, self.world) return 'SUCCESS - ' ''' Save the current joint states as a frame that we can return to ''' def save_joints_call(self, req): rospy.logwarn( 'Save frame does not check to see if your joint position already exists!' ) print self.q0 self.waypoint_manager.save_frame(self.q0) return 'SUCCESS - ' ''' Find any valid object that meets the requirements. Find a cartesian path or possibly longer path through joint space. ''' def smart_move_call(self, req): if not self.driver_status == 'SERVO': rospy.logerr('DRIVER -- Not in servo mode!') return 'FAILURE - not in servo mode' # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) # Find possible poses res = self.get_waypoints_srv.get_waypoints( req.obj_class, # object class to move to req.predicates, # predicates to match [req.pose], # offset/transform from each member of the class [req.name] # placeholder name ) if res is None: msg = 'FAILURE - no objects found that meet predicate conditions!' return msg (poses, names, objects) = res T_fwd = pm.fromMatrix(self.kdl_kin.forward(self.q0)) if not poses is None: msg = 'FAILURE - no valid objects found!' dists = [] Ts = [] for (pose, name, obj) in zip(poses, names, objects): # figure out which tf frame we care about tf_path = name.split('/') tf_frame = '' for part in tf_path: if len(part) > 0: tf_frame = part break if len(tf_frame) == 0: continue # try to move to the pose until one succeeds T_base_world = pm.fromTf( self.listener.lookupTransform(self.world, self.base_link, rospy.Time(0))) T = T_base_world.Inverse() * pm.fromMsg(pose) Ts.append(T) # TODO(cpaxton) update this to include rotation dists.append((T.p - T_fwd.p).Norm()) if len(Ts) == 0: msg = 'FAILURE - no objects found!' else: possible_goals = zip(dists, Ts, objects, names) possible_goals.sort() for (dist, T, obj, name) in possible_goals: rospy.logwarn("Trying to move to frame at distance %f" % (dist)) # plan to T (code, res) = self.planner.getPlan(T, self.q0, obj=obj) msg = self.send_and_publish_planning_result( res, acceleration, velocity) if msg[0:7] == 'SUCCESS': break return msg else: msg = 'FAILURE - no matching moves for specified predicates' return msg def set_goal(self, q): self.at_goal = False self.near_goal = False self.goal = pm.fromMatrix(self.kdl_kin.forward(q)) def send_and_publish_planning_result(self, res, acceleration, velocity): if (not res is None) and len( res.planned_trajectory.joint_trajectory.points) > 0: disp = DisplayTrajectory() disp.trajectory.append(res.planned_trajectory) disp.trajectory_start = res.trajectory_start self.display_pub.publish(disp) traj = res.planned_trajectory.joint_trajectory return self.send_trajectory(traj, acceleration, velocity, cartesian=False) else: rospy.logerr('DRIVER -- PLANNING failed') return 'FAILURE - not in servo mode' ''' Definitely do a planned motion. ''' def plan_to_pose_call(self, req): #rospy.loginfo('Recieved servo to pose request') #print req if self.driver_status == 'SERVO': T = pm.fromMsg(req.target) # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) # Send command pt = JointTrajectoryPoint() pose = pm.fromMsg(req.target) (code, res) = self.planner.getPlan(pose, self.q0) print "DONE PLANNING: " + str((code, res)) return self.send_and_publish_planning_result( res, acceleration, velocity) else: rospy.logerr('DRIVER -- not in servo mode!') return 'FAILURE - not in servo mode' ''' Send a whole joint trajectory message to a robot... that is listening to individual joint states. ''' def send_trajectory(self, traj, acceleration=0.5, velocity=0.5, cartesian=False): rospy.logerr( "Function 'send_trajectory' not implemented for base class!") return "FAILURE - running base class!" ''' Standard movement call. Tries a cartesian move, then if that fails goes into a joint-space move. ''' def servo_to_pose_call(self, req): if self.driver_status == 'SERVO': T = pm.fromMsg(req.target) # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) # inverse kinematics traj = self.planner.getCartesianMove(T, self.q0, self.base_steps, self.steps_per_meter) #if len(traj.points) == 0: # (code,res) = self.planner.getPlan(req.target,self.q0) # find a non-local movement # if not res is None: # traj = res.planned_trajectory.joint_trajectory # Send command if len(traj.points) > 0: rospy.logwarn("Robot moving to " + str(traj.points[-1].positions)) return self.send_trajectory(traj, acceleration, velocity, cartesian=False, linear=True) else: rospy.logerr('SIMPLE DRIVER -- IK failed') return 'FAILURE - not in servo mode' else: rospy.logerr('SIMPLE DRIVER -- Not in servo mode') return 'FAILURE - not in servo mode' ''' Standard move call. Make a joint space move to a destination. ''' def servo_to_joints_call(self, req): if self.driver_status == 'SERVO': # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) return 'FAILURE - not yet implemented!' else: rospy.logerr('SIMPLE DRIVER -- Not in servo mode') return 'FAILURE - not in servo mode' ''' set teach mode ''' def set_teach_mode_call(self, req): if req.enable == True: # self.rob.set_freedrive(True) self.driver_status = 'TEACH' return 'SUCCESS - teach mode enabled' else: # self.rob.set_freedrive(False) self.driver_status = 'IDLE' return 'SUCCESS - teach mode disabled' ''' send a single point ''' def send_q(self, pt, acceleration, velocity): pt = JointTrajectoryPoint() pt.positions = self.q0 self.pt_publisher.publish(pt) ''' activate servo mode ''' def set_servo_mode_call(self, req): if req.mode == 'SERVO': self.send_q(self.q0, 0.1, 0.1) self.driver_status = 'SERVO' return 'SUCCESS - servo mode enabled' elif req.mode == 'DISABLE': self.driver_status = 'IDLE' return 'SUCCESS - servo mode disabled' def shutdown_arm_call(self, req): self.driver_status = 'SHUTDOWN' pass ''' robot-specific logic to update state every "tick" ''' def handle_tick(self): rospy.logerr("Function 'handle_tick' not implemented for base class!") ''' call this when "spinning" to keep updating things ''' def tick(self): self.status_publisher.publish(self.driver_status) self.update_position() self.handle_tick() # publish TF messages to display frames self.waypoint_manager.publish_tf()
class YumiGelslimPybullet(object): """ Class for interfacing with Yumi in PyBullet with external motion planning, inverse kinematics, and forward kinematics, along with other helpers """ def __init__(self, yumi_pb, cfg, exec_thread=True, sim_step_repeat=10): """ Class constructor. Sets up internal motion planning interface for each arm, forward and inverse kinematics solvers, and background threads for updating the position of the robot. Args: yumi_pb (airobot Robot): Instance of PyBullet simulated robot, from airobot library cfg (YACS CfgNode): Configuration parameters exec_thread (bool, optional): Whether or not to start the background joint position control thread. Defaults to True. sim_step_repeat (int, optional): Number of simulation steps to take each time the desired joint position value is updated. Defaults to 10 """ self.cfg = cfg self.yumi_pb = yumi_pb self.sim_step_repeat = sim_step_repeat self.joint_lock = threading.RLock() self._both_pos = self.yumi_pb.arm.get_jpos() self._single_pos = {} self._single_pos['right'] = \ self.yumi_pb.arm.arms['right'].get_jpos() self._single_pos['left'] = \ self.yumi_pb.arm.arms['left'].get_jpos() self.moveit_robot = moveit_commander.RobotCommander() self.moveit_scene = moveit_commander.PlanningSceneInterface() self.moveit_planner = 'RRTConnectkConfigDefault' self.robot_description = '/robot_description' self.urdf_string = rospy.get_param(self.robot_description) self.mp_left = GroupPlanner('left_arm', self.moveit_robot, self.moveit_planner, self.moveit_scene, max_attempts=3, planning_time=5.0, goal_tol=0.5, eef_delta=0.01, jump_thresh=10) self.mp_right = GroupPlanner('right_arm', self.moveit_robot, self.moveit_planner, self.moveit_scene, max_attempts=3, planning_time=5.0, goal_tol=0.5, eef_delta=0.01, jump_thresh=10) self.fk_solver_r = KDLKinematics(URDF.from_parameter_server(), "yumi_body", "yumi_tip_r") self.fk_solver_l = KDLKinematics(URDF.from_parameter_server(), "yumi_body", "yumi_tip_l") self.num_ik_solver_r = trac_ik.IK('yumi_body', 'yumi_tip_r', urdf_string=self.urdf_string) self.num_ik_solver_l = trac_ik.IK('yumi_body', 'yumi_tip_l', urdf_string=self.urdf_string) self.ik_pos_tol = 0.001 # 0.001 working well with pulling self.ik_ori_tol = 0.01 # 0.01 working well with pulling self.execute_thread = threading.Thread(target=self._execute_both) self.execute_thread.daemon = True if exec_thread: self.execute_thread.start() self.step_sim_mode = False else: self.step_sim_mode = True def _execute_single(self): """ Background thread for controlling a single arm """ while True: self.joint_lock.acquire() self.yumi_pb.arm.set_jpos(self._both_pos, wait=True) self.joint_lock.release() time.sleep(0.01) def _execute_both(self): """ Background thread for controlling both arms """ while True: self.joint_lock.acquire() self.yumi_pb.arm.set_jpos(self._both_pos, wait=True) self.joint_lock.release() time.sleep(0.01) def update_joints(self, pos, arm=None): """ Setter function for external user to update the target joint values for the arms. If manual step mode is on, this function also takes simulation steps. Args: pos (list): Desired joint angles, either for both arms or a single arm arm (str, optional): Which arm to update the joint values for either 'right', or 'left'. If none, assumed updating for both. Defaults to None. Raises: ValueError: Bad arm name """ if arm is None: self.joint_lock.acquire() self._both_pos = pos self.joint_lock.release() elif arm == 'right': both_pos = list(pos) + self.yumi_pb.arm.arms['left'].get_jpos() self.joint_lock.acquire() self._both_pos = both_pos self.joint_lock.release() elif arm == 'left': both_pos = self.yumi_pb.arm.arms['right'].get_jpos() + list(pos) self.joint_lock.acquire() self._both_pos = both_pos self.joint_lock.release() else: raise ValueError('Arm not recognized') self.yumi_pb.arm.set_jpos(self._both_pos, wait=False) if self.step_sim_mode: for _ in range(self.sim_step_repeat): # step_simulation() self.yumi_pb.pb_client.stepSimulation() def compute_fk(self, joints, arm='right'): """ Forward kinematics calculation. Args: joints (list): Joint configuration, should be len() = DOF of a single arm (7 for Yumi) arm (str, optional): Which arm to compute FK for. Defaults to 'right'. Returns: PoseStamped: End effector pose corresponding to the FK solution for the input joint configuation """ if arm == 'right': matrix = self.fk_solver_r.forward(joints) else: matrix = self.fk_solver_l.forward(joints) translation = transformations.translation_from_matrix(matrix) quat = transformations.quaternion_from_matrix(matrix) ee_pose_array = np.hstack((translation, quat)) ee_pose = util.convert_pose_type(ee_pose_array, type_out='PoseStamped', frame_out='yumi_body') return ee_pose def compute_ik(self, pos, ori, seed, arm='right', solver='trac'): """ Inverse kinematics calcuation Args: pos (list): Desired end effector position [x, y, z] ori (list): Desired end effector orientation (quaternion), [x, y, z, w] seed (list): Initial solution guess to IK calculation. Returned solution will be near the seed. arm (str, optional): Which arm to compute IK for. Defaults to 'right'. solver (str, optional): Which IK solver to use. Defaults to 'trac'. Returns: list: Configuration space solution corresponding to the desired end effector pose. len() = DOF of single arm """ if arm != 'right' and arm != 'left': arm = 'right' if arm == 'right': if solver == 'trac': sol = self.num_ik_solver_r.get_ik( seed, pos[0], pos[1], pos[2], ori[0], ori[1], ori[2], ori[3], self.ik_pos_tol, self.ik_pos_tol, self.ik_pos_tol, self.ik_ori_tol, self.ik_ori_tol, self.ik_ori_tol) else: sol = self.yumi_pb.arm.compute_ik(pos, ori, arm='right') elif arm == 'left': if solver == 'trac': sol = self.num_ik_solver_l.get_ik( seed, pos[0], pos[1], pos[2], ori[0], ori[1], ori[2], ori[3], self.ik_pos_tol, self.ik_pos_tol, self.ik_pos_tol, self.ik_ori_tol, self.ik_ori_tol, self.ik_ori_tol) else: sol = self.yumi_pb.arm.compute_ik(pos, ori, arm='left') return sol def unify_arm_trajectories(self, left_arm, right_arm, tip_poses): """ Function to return a right arm and left arm trajectory of the same number of points, where the index of the points that align with the goal cartesian poses of each arm are the same for both trajectories Args: left_arm (JointTrajectory): left arm joint trajectory from left move group after calling compute_cartesian_path right_arm (JointTrajectory): right arm joint trajectory from right move group after calling compute_cartesian_path tip_poses (list): list of desired end effector poses for both arms for a particular segment of a primitive plan Returns: dict: Dictionary of combined trajectories in different formats. Keys for each arm, 'right' and 'left', which each are themselves dictionary. Deeper keys --- 'fk': Cartesian path numpy array, unaligned 'joints': C-space path numpy array, unaligned 'aligned_fk': Cartesian path numpy array, aligned 'aligned_joints': C-space path numpy array, aligned 'closest_inds': Indices of each original path corresponding to the closest value in the other arms trajectory """ # find the longer trajectory long_traj = 'left' if len(left_arm.points) > len(right_arm.points) \ else 'right' # make numpy array of each arm joint trajectory for each comp left_arm_joints_np = np.zeros((len(left_arm.points), 7)) right_arm_joints_np = np.zeros((len(right_arm.points), 7)) # make numpy array of each arm pose trajectory, based on fk left_arm_fk_np = np.zeros((len(left_arm.points), 7)) right_arm_fk_np = np.zeros((len(right_arm.points), 7)) for i, point in enumerate(left_arm.points): left_arm_joints_np[i, :] = point.positions pose = self.compute_fk(point.positions, arm='left') left_arm_fk_np[i, :] = util.pose_stamped2list(pose) for i, point in enumerate(right_arm.points): right_arm_joints_np[i, :] = point.positions pose = self.compute_fk(point.positions, arm='right') right_arm_fk_np[i, :] = util.pose_stamped2list(pose) closest_left_inds = [] closest_right_inds = [] # for each tip_pose, find the index in the longer trajectory that # most closely matches the pose (using fk) for i in range(len(tip_poses)): r_waypoint = util.pose_stamped2list(tip_poses[i][1]) l_waypoint = util.pose_stamped2list(tip_poses[i][0]) r_pos_diffs = util.pose_difference_np( pose=right_arm_fk_np, pose_ref=np.array(r_waypoint))[0] l_pos_diffs = util.pose_difference_np( pose=left_arm_fk_np, pose_ref=np.array(l_waypoint))[0] # r_pos_diffs, r_ori_diffs = util.pose_difference_np( # pose=right_arm_fk_np, pose_ref=np.array(r_waypoint)) # l_pos_diffs, l_ori_diffs = util.pose_difference_np( # pose=left_arm_fk_np, pose_ref=np.array(l_waypoint)) r_index = np.argmin(r_pos_diffs) l_index = np.argmin(l_pos_diffs) # r_index = np.argmin(r_pos_diffs + r_ori_diffs) # l_index = np.argmin(l_pos_diffs + l_ori_diffs) closest_right_inds.append(r_index) closest_left_inds.append(l_index) # Create a new trajectory for the shorter trajectory, that is the same # length as the longer trajectory. if long_traj == 'left': new_right = np.zeros((left_arm_joints_np.shape)) prev_r_ind = 0 prev_new_ind = 0 for i, r_ind in enumerate(closest_right_inds): # Put the joint values from the short # trajectory at the indices corresponding to the path waypoints # at the corresponding indices found for the longer trajectory new_ind = closest_left_inds[i] new_right[new_ind, :] = right_arm_joints_np[r_ind, :] # For the missing values in between the joint waypoints, # interpolate to fill the trajectory # if new_ind - prev_new_ind > -1: interp = np.linspace(right_arm_joints_np[prev_r_ind, :], right_arm_joints_np[r_ind], num=new_ind - prev_new_ind) new_right[prev_new_ind:new_ind, :] = interp prev_r_ind = r_ind prev_new_ind = new_ind aligned_right_joints = new_right aligned_left_joints = left_arm_joints_np else: new_left = np.zeros((right_arm_joints_np.shape)) prev_l_ind = 0 prev_new_ind = 0 for i, l_ind in enumerate(closest_left_inds): new_ind = closest_right_inds[i] new_left[new_ind, :] = left_arm_joints_np[l_ind, :] interp = np.linspace(left_arm_joints_np[prev_l_ind, :], left_arm_joints_np[l_ind], num=new_ind - prev_new_ind) new_left[prev_new_ind:new_ind, :] = interp prev_l_ind = l_ind prev_new_ind = new_ind aligned_right_joints = right_arm_joints_np aligned_left_joints = new_left # get aligned poses of the end effector as well aligned_right_fk = np.zeros((aligned_right_joints.shape[0], 7)) aligned_left_fk = np.zeros((aligned_left_joints.shape[0], 7)) for i in range(aligned_right_joints.shape[0]): pose_r = self.compute_fk(aligned_right_joints[i, :], arm='right') aligned_right_fk[i, :] = util.pose_stamped2list(pose_r) pose_l = self.compute_fk(aligned_left_joints[i, :], arm='left') aligned_left_fk[i, :] = util.pose_stamped2list(pose_l) unified = {} unified['right'] = {} unified['right']['fk'] = right_arm_fk_np unified['right']['joints'] = right_arm_joints_np unified['right']['aligned_fk'] = aligned_right_fk unified['right']['aligned_joints'] = aligned_right_joints unified['right']['inds'] = closest_right_inds unified['left'] = {} unified['left']['fk'] = left_arm_fk_np unified['left']['joints'] = left_arm_joints_np unified['left']['aligned_fk'] = aligned_left_fk unified['left']['aligned_joints'] = aligned_left_joints unified['left']['inds'] = closest_left_inds return unified def tip_to_wrist(self, tip_poses): """ Transform a pose from the Yumi Gelslim tip to the wrist joint Args: tip_poses (dict): Dictionary of PoseStamped values corresponding to each arm, keyed by 'right' and 'left' Returns: dict: Keyed by 'right' and 'left', values are PoseStamped """ tip_to_wrist = util.list2pose_stamped(self.cfg.TIP_TO_WRIST_TF, '') world_to_world = util.unit_pose() wrist_left = util.convert_reference_frame(tip_to_wrist, world_to_world, tip_poses['left'], "yumi_body") wrist_right = util.convert_reference_frame(tip_to_wrist, world_to_world, tip_poses['right'], "yumi_body") wrist_poses = {} wrist_poses['right'] = wrist_right wrist_poses['left'] = wrist_left return wrist_poses def wrist_to_tip(self, wrist_poses): """ Transform a pose from the Yumi wrist joint to the Gelslim tip to the Args: wrist_poses (dict): Dictionary of PoseStamped values corresponding to each arm, keyed by 'right' and 'left' Returns: dict: Keyed by 'right' and 'left', values are PoseStamped """ wrist_to_tip = util.list2pose_stamped(self.cfg.WRIST_TO_TIP_TF, '') tip_left = util.convert_reference_frame(wrist_to_tip, util.unit_pose(), wrist_poses['left'], "yumi_body") tip_right = util.convert_reference_frame(wrist_to_tip, util.unit_pose(), wrist_poses['right'], "yumi_body") tip_poses = {} tip_poses['right'] = tip_right tip_poses['left'] = tip_left return tip_poses def is_in_contact(self, object_id): """ Checks whether or not robot is in contact with a particular object Args: object_id (int): Pybullet object ID of object contact is checked with Returns: dict: Keyed by 'right' and 'left', values are bools. True means arm 'right/left' is in contact, else False """ # r_pts = p.getContactPoints( # bodyA=self.yumi_pb.arm.robot_id, bodyB=object_id, linkIndexA=12, physicsClientId=pb_util.PB_CLIENT) # l_pts = p.getContactPoints( # bodyA=self.yumi_pb.arm.robot_id, bodyB=object_id, linkIndexA=25, physicsClientId=pb_util.PB_CLIENT) r_pts = p.getContactPoints( bodyA=self.yumi_pb.arm.robot_id, bodyB=object_id, linkIndexA=self.cfg.RIGHT_GEL_ID, physicsClientId=self.yumi_pb.pb_client.get_client_id()) l_pts = p.getContactPoints( bodyA=self.yumi_pb.arm.robot_id, bodyB=object_id, linkIndexA=self.cfg.LEFT_GEL_ID, physicsClientId=self.yumi_pb.pb_client.get_client_id()) r_contact_bool = 0 if len(r_pts) == 0 else 1 l_contact_bool = 0 if len(l_pts) == 0 else 1 contact_bool = {} contact_bool['right'] = r_contact_bool contact_bool['left'] = l_contact_bool return contact_bool def get_jpos(self, arm=None): """ Getter function for getting the robot's joint positions Args: arm (str, optional): Which arm to get the position for. Defaults to None, if None will return position of both arms. Returns: list: Joint positions, len() = DOF of single arm """ if arm is None: jpos = self.yumi_pb.arm.get_jpos() elif arm == 'left': jpos = self.yumi_pb.arm.arms['left'].get_jpos() elif arm == 'right': jpos = self.yumi_pb.arm.arms['right'].get_jpos() else: raise ValueError('Arm not recognized') return jpos def get_ee_pose(self, arm='right'): """ Getter function for getting the robot end effector pose Args: arm (str, optional): Which arm to get the EE pose for. Defaults to 'right'. Returns: 4-element tuple containing - np.ndarray: x, y, z position of the EE (shape: :math:`[3,]`) - np.ndarray: quaternion representation of the EE orientation (shape: :math:`[4,]`) - np.ndarray: rotation matrix representation of the EE orientation (shape: :math:`[3, 3]`) - np.ndarray: euler angle representation of the EE orientation (roll, pitch, yaw with static reference frame) (shape: :math:`[3,]`) """ if arm == 'right': ee_pose = self.yumi_pb.arm.get_ee_pose(arm='right') elif arm == 'left': ee_pose = self.yumi_pb.arm.get_ee_pose(arm='left') else: raise ValueError('Arm not recognized') return ee_pose def get_palm_y_normals(self, palm_poses=None, *args): """ Gets the updated world frame normal direction of the palms """ normal_y = util.list2pose_stamped([0, 1, 0, 0, 0, 0, 1]) normal_y_poses_world = {} wrist_poses = {} for arm in ['right', 'left']: if palm_poses is None: wrist_pos_world = self.get_ee_pose(arm=arm)[0].tolist() wrist_ori_world = self.get_ee_pose(arm=arm)[1].tolist() wrist_poses[arm] = util.list2pose_stamped(wrist_pos_world + wrist_ori_world) else: wrist_poses[arm] = palm_poses[arm] tip_poses = self.wrist_to_tip(wrist_poses) normal_y_poses_world['right'] = util.transform_pose( normal_y, tip_poses['right']) normal_y_poses_world['left'] = util.transform_pose( normal_y, tip_poses['left']) return normal_y_poses_world def get_palm_z_normals(self, palm_poses=None, *args): """ Gets the updated world frame normal direction of the palms """ normal_z = util.list2pose_stamped([0, 0, 1, 0, 0, 0, 1]) normal_z_poses_world = {} wrist_poses = {} for arm in ['right', 'left']: if palm_poses is None: wrist_pos_world = self.get_ee_pose(arm=arm)[0].tolist() wrist_ori_world = self.get_ee_pose(arm=arm)[1].tolist() wrist_poses[arm] = util.list2pose_stamped(wrist_pos_world + wrist_ori_world) else: wrist_poses[arm] = palm_poses[arm] tip_poses = self.wrist_to_tip(wrist_poses) normal_z_poses_world['right'] = util.transform_pose( normal_z, tip_poses['right']) normal_z_poses_world['left'] = util.transform_pose( normal_z, tip_poses['left']) return normal_z_poses_world def get_current_tip_poses(self): wrist_poses = {} for arm in ['right', 'left']: wrist_pos_world = self.get_ee_pose(arm=arm)[0].tolist() wrist_ori_world = self.get_ee_pose(arm=arm)[1].tolist() wrist_poses[arm] = util.list2pose_stamped(wrist_pos_world + wrist_ori_world) tip_poses = self.wrist_to_tip(wrist_poses) return tip_poses
def build_tree(iter, treeA, treeB, edgesA, edgesB, plot, kdl_kin): i = 0 while i < iter: # print(i) jointsA = np.random.rand(1, 7)[0] * [2.5, 2.5, 4, 1.5, 4, 2.5, 4] - [ 1.25, 1.25, 2.0, .75, 2.0, .75, 2.0 ] velA = np.random.rand(1, 7)[0] * [3.0, 3.0, 3.0, 3.0, 6.0, 3.0, 6.0 ] - [1.5, 1.5, 1.5, 1.5, 2, 1.5, 2] node = nearest_neighbor(jointsA, velA, treeA, i) treeA = insert_vertex(node, treeA, jointsA, velA, i, 1) edgesA = insert_edge(edgesA, node, i) jointsB = np.random.rand(1, 7)[0] * [2.5, 2.5, 4, 1.5, 4, 2.5, 4] - [ 1.25, 1.25, 2.0, .75, 2.0, .75, 2.0 ] velB = np.random.rand(1, 7)[0] * [3.0, 3.0, 3.0, 3.0, 6.0, 3.0, 6.0 ] - [1.5, 1.5, 1.5, 1.5, 2, 1.5, 2] node = nearest_neighbor(jointsB, velB, treeB, i) treeB = insert_vertex(node, treeB, jointsB, velB, i, 1) edgesB = insert_edge(edgesB, node, i) result = connect_trees(treeA, treeB, i, kdl_kin) # print(result[0],i) if (result[0]): print "iterated up to: " print i break i = i + 1 iterations = i if (plot): # plt.close('all') # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.scatter(treeA[0,0],treeA[0,1],treeA[0,2],'r') # ax.plot(treeA[0:i+2,0],treeA[0:i+2,1],treeA[0:i+2,2],'r.') # ax.scatter(treeB[0,0],treeB[0,1],treeB[0,2],'b') # ax.plot(treeB[0:i+2,0],treeB[0:i+2,1],treeB[0:i+2,2],'b.') # plt.show(block=False) robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') pos3A = np.empty((iterations, 3)) for i in range(iterations): pos3A[i, :] = kdl_kin.forward(treeA[i, :7])[0:3, 3].transpose() pos3B = np.empty((iterations, 3)) for i in range(iterations): pos3B[i, :] = kdl_kin.forward(treeB[i, :7])[0:3, 3].transpose() fig = plt.figure(1) plt.hold(False) # ax = fig.gca(projection='3d') plt.plot(pos3A[:, 1], pos3A[:, 2], 'r.') plt.hold(True) plt.plot(pos3A[0, 1], pos3A[0, 2], 'r.', markersize=15) plt.plot(pos3B[:, 1], pos3B[:, 2], 'b.') plt.plot(pos3B[0, 1], pos3B[0, 2], 'b.', markersize=15) plt.xlabel('EE y-coordinate') plt.ylabel('EE z-coordinate') plt.title('RRT in end-effector space') # plt.show(block=False) if (result[0]): indA = result[1] indB = result[2] # if (plot): # ax.scatter([treeA[indA,0], treeB[indB,0]],[treeA[indA,1], treeB[indB,1]],[treeA[indA,2], treeB[indB,2]],'g') else: indA = 0 indB = 0 if (plot): # plt.xlim([-1.5, 1.5]) # plt.ylim([-1.5, 1.5]) for k in range(iterations + 1): edge1 = kdl_kin.forward(treeA[edgesA[k, 0], :7])[0:3, 3].transpose() edge2 = kdl_kin.forward(treeA[edgesA[k, 1], :7])[0:3, 3].transpose() plt.plot([edge1[0, 1], edge2[0, 1]], [edge1[0, 2], edge2[0, 2]], 'r', linewidth=3) edge1 = kdl_kin.forward(treeB[edgesB[k, 0], :7])[0:3, 3].transpose() edge2 = kdl_kin.forward(treeB[edgesB[k, 1], :7])[0:3, 3].transpose() plt.plot([edge1[0, 1], edge2[0, 1]], [edge1[0, 2], edge2[0, 2]], 'b', linewidth=3) # plt.show(block=False) # raw_input('Enter...') return treeA[0:iterations + 2, :], treeB[0:iterations + 2, :], edgesA[0:iterations + 1, :], edgesB[0:iterations + 1, :], indA, indB
def execute_move(self, pos): rospy.loginfo('moving') # Read in pose data q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z] p =[[pos.position.x],[pos.position.y],[pos.position.z]] # Convert quaternion data to rotation matrix R = quat_to_so3(q); # Form transformation matrix robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') # Create seed with current position q0 = kdl_kin.random_joint_angles() limb_interface = baxter_interface.limb.Limb('right') current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()] for ind in range(len(q0)): q0[ind] = current_angles[ind] pose = kdl_kin.forward(q0) Xstart = copy(np.asarray(pose)) pose[0:3,0:3] = R pose[0:3,3] = p Xend = copy(np.asarray(pose)) # Compute straight-line trajectory for path N = 500 Xlist = CartesianTrajectory(Xstart, Xend, 1, N, 5) thList = np.empty((N,7)) thList[0] = q0; for i in range(N-1): # Solve for joint angles seed = 0 q_ik = kdl_kin.inverse(Xlist[i+1], thList[i]) while q_ik == None: seed += 0.3 q_ik = kdl_kin.inverse(pose, q0+seed) thList[i+1] = q_ik # rospy.loginfo(q_ik) # # Solve for joint angles # seed = 0.3 # q_ik = kdl_kin.inverse(pose, q0+seed) # while q_ik == None: # seed += 0.3 # q_ik = kdl_kin.inverse(pose, q0+seed) # rospy.loginfo(q_ik) # q_list = JointTrajectory(q0,q_ik,1,100,5) # for q in q_list: # Format joint angles as limb joint angle assignment angles = limb_interface.joint_angles() angle_count = 0 for ind, joint in enumerate(limb_interface.joint_names()): # if fabs(angles[joint] - q_ik[ind]) < .05: # angle_count += 1 angles[joint] = q_ik[ind] # rospy.loginfo(angles) rospy.sleep(.003) # Send joint move command rospy.loginfo('move to object') # rospy.loginfo(angle_count) # if angle_count > 4: # nothing = True; # else: limb_interface.set_joint_position_speed(.3) limb_interface.set_joint_positions(angles) self._done = True print('Done')
def get_jaco_kinematics(self): kin = KDLKinematics(self.urdf, self.base_link, self.end_link) return kin
else: print "The object detection result is not valid!" object_found = False # the names of the base_link and end_link according to the urdf model base_link = "iiwa_0_link" end_link = "iiwa_adapter" # initialize a node rospy.init_node("manipulator_mover") robot = URDF.from_xml_file( "/home/davoud/kuka_ws/src/kuka/kuka/iiwa/iiwa_description/urdf/iiwa.urdf") tree = kdl_tree_from_urdf_model(robot) kdl_kin = KDLKinematics(robot, base_link, end_link) # initialize a publisher for publishing the needed data for object pose saving in a file # file_data_pub = rospy.Publisher("/vrep_ros_communication/file_data", FileStorageInf, queue_size=1) # actionlib commands manipulator_client = actionlib.SimpleActionClient( "/iiwa/follow_joint_trajectory", FollowJointTrajectoryAction) manipulator_client.wait_for_server() trajectory = JointTrajectory() trajectory.joint_names = joint_names trajectory.points.append(JointTrajectoryPoint()) # subscribe to "/joint_states" topic which is published by kuka. rospy.Subscriber("/joint_states", JointState, kuka_inf_callback)
def __init__(self): limb = read_parameter('~limb', 'right') if limb not in ['right', 'left']: rospy.logerr('Unknown limb name [%s]' % limb) return # Read the controllers parameters gains = read_parameter('~gains', dict()) self.kp = joint_list_to_kdl(gains['Kp']) self.kd = joint_list_to_kdl(gains['Kd']) self.publish_rate = read_parameter('~publish_rate', 500) self.frame_id = read_parameter('~frame_id', 'base') self.timeout = read_parameter('~timeout', 0.02) # seconds # Set-up baxter interface self.arm_interface = baxter_interface.Limb(limb) # set_joint_torques must be commanded at a rate great than the timeout specified by set_command_timeout. # If the timeout is exceeded before a new set_joint_velocities command is received, the controller will switch # modes back to position mode for safety purposes. self.arm_interface.set_command_timeout(self.timeout) # Baxter kinematics self.urdf = URDF.from_parameter_server(key='robot_description') self.tip_link = '%s_gripper' % limb self.kinematics = KDLKinematics(self.urdf, self.frame_id, self.tip_link) self.fk_vel_solver = PyKDL.ChainFkSolverVel_recursive( self.kinematics.chain) # Adjust the publish rate of baxter's state joint_state_pub_rate = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16) # Get the joint names and limits self.joint_names = self.arm_interface.joint_names() self.num_joints = len(self.joint_names) self.lower_limits = np.zeros(self.num_joints) self.upper_limits = np.zeros(self.num_joints) # KDL joint may be in a different order than expected kdl_lower_limits, kdl_upper_limits = self.kinematics.get_joint_limits() for i, name in enumerate(self.kinematics.get_joint_names()): if name in self.joint_names: idx = self.joint_names.index(name) self.lower_limits[idx] = kdl_lower_limits[i] self.upper_limits[idx] = kdl_upper_limits[i] self.middle_values = (self.upper_limits + self.lower_limits) / 2.0 # Move the arm to a neutral position #~ self.arm_interface.move_to_neutral(timeout=10.0) #~ neutral_pos = [-math.pi/4, 0.0, 0.0, 0.0, 0.0, math.pi/2, -math.pi/2] neutral_pos = [1.0, -0.57, -1.2, 1.3, 0.86, 1.4, -0.776] #~ neutral_pos = [pi/4, -pi/3, 0.0, pi/2, 0.0, pi/3, 0.0] neutral_cmd = dict() for i, name in enumerate(self.joint_names): if name in ['left_s0', 'left_w2']: neutral_cmd[name] = neutral_pos[i] * -1.0 else: neutral_cmd[name] = neutral_pos[i] self.arm_interface.move_to_joint_positions(neutral_cmd, timeout=10.0) # Baxter faces self.limb = limb self.face = FaceImage() self.face.set_image('happy') # Set-up publishers/subscribers self.suppress_grav_comp = rospy.Publisher( '/robot/limb/%s/suppress_gravity_compensation' % limb, Empty) joint_state_pub_rate.publish(int(self.publish_rate)) self.feedback_pub = rospy.Publisher('/takktile/force_feedback', Wrench) rospy.Subscriber('/baxter/%s_ik_command' % limb, PoseStamped, self.ik_command_cb) self.gripper_closed = False rospy.Subscriber('/takktile/calibrated', Touch, self.takktile_cb) rospy.Subscriber('/robot/end_effector/%s_gripper/state' % limb, EndEffectorState, self.end_effector_cb) rospy.loginfo('Running Cartesian controller for the %s arm' % limb) # Start torque controller timer self.cart_command = None self.torque_controller_timer = rospy.Timer( rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb) # Shutdown hookup to clean-up before killing the script rospy.on_shutdown(self.shutdown)
class Robot(object): def __init__(self, sim=True): f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r') robot = URDF.from_xml_string(f.read()) # parsed URDF # robot = URDF.from_parameter_server() self.kin = KDLKinematics(robot, 'base', 'mconn') # Selection of end-effector parameters for WidowX # x, y, z, yaw, pitch self.cart_from_matrix = lambda T: np.array([ T[0, 3], T[1, 3], T[2, 3], math.atan2(T[1, 0], T[0, 0]), -math.asin(T[2, 0])]) self.home = np.array([0.05, 0, 0.25, 0, 0]) # home end-effector pose self.q = np.array([0, 0, 0, 0, 0]) # joint angles self.dt = 0.05 # algorithm time step self.sim = sim # true if virtual robot def publish(eventStop): # for arbotix pub1 = rospy.Publisher("q1/command", Float64, queue_size=5) pub2 = rospy.Publisher("q2/command", Float64, queue_size=5) pub3 = rospy.Publisher("q3/command", Float64, queue_size=5) pub4 = rospy.Publisher("q4/command", Float64, queue_size=5) pub5 = rospy.Publisher("q5/command", Float64, queue_size=5) # for visualization jointPub = rospy.Publisher( "/joint_states", JointState, queue_size=5) jmsg = JointState() jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5'] while not rospy.is_shutdown() and not eventStop.is_set(): jmsg.header.stamp = rospy.Time.now() jmsg.position = self.q if self.sim: jointPub.publish(jmsg) pub1.publish(self.q[0]) pub2.publish(self.q[1]) pub3.publish(self.q[2]) pub4.publish(self.q[3]) pub5.publish(self.q[4]) eventStop.wait(self.dt) rospy.init_node("robot") eventStop = threading.Event() threadJPub = threading.Thread(target=publish, args=(eventStop,)) threadJPub.daemon = True threadJPub.start() # Update joint angles in a background process def act_gripper(self, grip): # Actuate gripper given gripper position pub = rospy.Publisher('q6/command', Float64, queue_size=5) rospy.sleep(1) pub.publish(grip) rospy.sleep(1) def forward(self, q=None): # End-effector transformation matrix, given joint angles if q is None: q = self.q return(self.kin.forward(q)) def cart(self, q=None): # Cartesian coordinates [x, y, z, r1, r2, r3], given joint angles if q is None: q = self.q return(self.cart_from_matrix(self.forward(q))) def jacobian(self, q=None): # Inertial jacobian if q is None: q = self.q eps = 1e-3 s0 = np.array(self.cart(q)) dof = len(self.q) J = np.array(dof * [dof * [0.0]]) for k in range(0, dof): dq = np.zeros(dof) dq[k] = eps J[:, k] = (self.cart(q + dq) - s0) / eps return J def move(self, s1, speed=1): def plan(s1): # return tuples of joint coordinates to be moved at every time step # Error function to find the zero def error_function(q): return ( np.array(self.cart(q)) - np.array(s1)) x, _, ler, message = scipy.optimize.fsolve( error_function, 0*self.q, full_output=True) if not ler == 1: print('No IK solution found') q1 = self.q else: q1 = (x + math.pi) % (2 * math.pi) - math.pi # q1 = x # break trajectory to intermediate points q0 = self.q # initial joint angles s0 = self.cart(q0) # initial end-effector pose t1 = np.max(np.abs(q1 - q0)) / speed # time to reach goal time = np.arange(0, t1, self.dt) + self.dt # time array dof = len(self.q) # Trajectory matrix qq = np.array([np.interp(time, [0, t1], [q0[k], q1[k]]) for k in range(dof)]).transpose() return qq traj = plan(s1) rate = rospy.Rate(1.0 / self.dt) for joints in traj: self.q = joints rate.sleep() def move2(self, s1): # Not used speed = 1.5 TOL = 1e-8 MAX_VEL = 0.2 t0 = rospy.Time.now() while rospy.Time.now() - t0 < rospy.Duration(5): ds = s1 - self.cart() dev = np.linalg.norm(ds) if dev < TOL: print("Success. Deviation: ") print(dev) return J = self.jacobian() dq = np.linalg.solve(J, ds) if max(abs(dq)) > MAX_VEL: dq = dq * MAX_VEL / max(abs(dq)) alfa = speed * self.dt * (rospy.Time.now() - t0).to_sec()**1.5 self.q = self.q + dq * alfa rospy.sleep(self.dt) print("Failure. Deviation: ") print(dev) def reset(self): self.move(self.cart(self.q*0))
class Mission: def __init__(self, name): self.name = name self.timer = 0 # the names of the base_link and end_link according to the urdf model self.base_link = "iiwa_0_link" self.end_link = "iiwa_adapter" # initialize a node rospy.init_node("mission_node") self.robot = URDF.from_xml_file( "/home/davoud/kuka_ws/src/kuka/kuka/iiwa/iiwa_description/urdf/iiwa.urdf" ) self.tree = kdl_tree_from_urdf_model(self.robot) self.kdl_kin = KDLKinematics(self.robot, self.base_link, self.end_link) # you can use one of these methods: # 1: Jacobian Transpose Method (JTM) # 2: Jacobian Pseudoinverse Method (JPM) # 3: Damped Least Squares Method (DLSM) self.used_method = "DLSM" self.landa = 0.1 # tf broadcaster self.br = tf.TransformBroadcaster() # current position of the end effector self.end_eff_current_pose = np.empty((1, 7)) # transformation matrix between end_effector and camera self.end_cam_T = np.empty((4, 4)) self.set_end_cam_T() # transformation matrix between end_effector and gripper self.end_grip_T = np.empty((4, 4)) self.set_end_grip_T() # transformation matrix between gripper and target pose self.grip_target_T = np.empty((4, 4)) self.set_grip_target_T() # transformation matrix between base and end effector self.base_end_T = np.empty((4, 4)) # transformation matrix between base and target self.base_target_T = np.empty((4, 4)) # needed variables for actionlib self.joint_names = [ "joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7" ] # actionlib commands self.manipulator_client = actionlib.SimpleActionClient( "/iiwa/follow_joint_trajectory", FollowJointTrajectoryAction) self.manipulator_client.wait_for_server() self.trajectory = JointTrajectory() self.trajectory.joint_names = self.joint_names self.trajectory.points.append(JointTrajectoryPoint()) # path of end_effector self.ee_path = MarkerArray() # path of object self.object_path = MarkerArray() # initializ a publisher to publish the marker of detected objet to rviz self.marker_object_in_base_pub = rospy.Publisher( "marker_object_in_base", Marker, queue_size=1) self.marker_object_in_cam_pub = rospy.Publisher("marker_object_in_cam", Marker, queue_size=1) self.marker_eff_path_pub = rospy.Publisher("marker_eff_path", MarkerArray, queue_size=1) self.marker_obj_path_pub = rospy.Publisher("marker_obj_path", MarkerArray, queue_size=1) # initialize the service client for the gripper rospy.wait_for_service("/sdh_action") self.joint_config_client = rospy.ServiceProxy("/sdh_action", SDHAction) # subscribe to "/joint_states" topic which is published by kuka. rospy.Subscriber("/joint_states", JointState, self.kuka_inf_cb) # subscribe to "/sphere_coefs" topic which is published by ball detector using color and distance filters rospy.Subscriber("/sphere_coefs", Float32MultiArray, self.marker_inf_cb, queue_size=1) rospy.spin() def kuka_inf_cb(self, data): if data._connection_header["callerid"] != "/kuka_manager": return self.end_eff_current_pose = data.position self.set_base_end_T() def marker_inf_cb(self, data): # in order to filter the inamissible (0.0, 0.0, 0.0, -inf) detections if data.data[0:3] == (0.0, 0.0, 0.0): # print "inadmissible input!" return vec3 = Vector3() vec3.x = data.data[0] vec3.y = data.data[1] vec3.z = data.data[2] ball_pose = np.array([vec3.x, vec3.y, vec3.z]) for i in range(3): if np.isnan(ball_pose[i]): return cam_object_T = self.set_cam_object_T(ball_pose) cam_object_R = tr.identity_matrix() cam_object_R[0:3, 0:3] = cam_object_T[0:3, 0:3] cam_object_q = tr.quaternion_from_matrix(cam_object_R) self.br.sendTransform( (cam_object_T[0, 3], cam_object_T[1, 3], cam_object_T[2, 3]), cam_object_q, rospy.Time.now(), "object_in_cam_frame", "camera_rgb_optical_frame") # publishing the object marker to rviz for object_in_cam det_marker = Marker() det_marker.header.frame_id = "camera_rgb_optical_frame" det_marker.header.stamp = rospy.Time.now() det_marker.type = Marker.SPHERE det_marker.pose.position.x = cam_object_T[0, 3] det_marker.pose.position.y = cam_object_T[1, 3] det_marker.pose.position.z = cam_object_T[2, 3] det_marker.pose.orientation.x = 0 det_marker.pose.orientation.y = 0 det_marker.pose.orientation.z = 0 det_marker.pose.orientation.w = 1 det_marker.scale.x = 0.05 det_marker.scale.y = 0.05 det_marker.scale.z = 0.05 det_marker.color.a = 1.0 det_marker.color.r = 1.0 det_marker.color.g = 0.0 det_marker.color.b = 0.0 self.marker_object_in_cam_pub.publish(det_marker) base_object_T = np.dot(np.dot(self.base_end_T, self.end_cam_T), cam_object_T) # in order to assign object the same orientation as target frame base_object_T[0:3, 0:3] = self.base_target_T[0:3, 0:3] final_err = self.error_computation(base_object_T, self.base_target_T) # target thresholds: trans_th = [0.05, 0.05, 0.05] rot_th = [15 * np.pi / 180, 15 * np.pi / 180, 15 * np.pi / 180] print "timer: ", self.timer if self.timer > 5: print "goal reached ............................" self.gripper_final_action() if np.abs(final_err[0]) < trans_th[0] and np.abs( final_err[1]) < trans_th[1] and np.abs( final_err[2]) < trans_th[2]: if np.abs(final_err[3]) < rot_th[0] and np.abs( final_err[4]) < rot_th[1] and np.abs( final_err[5]) < rot_th[2]: self.timer += 1 return else: self.timer = 0 print "error :", final_err else: self.timer = 0 print "error :", final_err # use of error_gains: error_gains = np.reshape(np.array([1, 1, 1, 1, 1, 1]), (6, 1)) final_err2 = np.multiply(final_err, error_gains) # error value type 2 end_object_T = np.dot(self.end_cam_T, cam_object_T) end_target_T = np.dot(self.end_grip_T, self.grip_target_T) object_target_T = np.dot(end_object_T, np.linalg.inv(end_target_T)) # print "error style 2: ", object_target_T # publishing the object marker to rviz det_marker = Marker() det_marker.header.frame_id = "iiwa_base_link" det_marker.header.stamp = rospy.Time.now() det_marker.type = Marker.SPHERE det_marker.pose.position.x = base_object_T[0, 3] det_marker.pose.position.y = base_object_T[1, 3] det_marker.pose.position.z = base_object_T[2, 3] det_marker.pose.orientation.x = 0 det_marker.pose.orientation.y = 0 det_marker.pose.orientation.z = 0 det_marker.pose.orientation.w = 1 det_marker.scale.x = 0.05 det_marker.scale.y = 0.05 det_marker.scale.z = 0.05 det_marker.color.a = 1.0 det_marker.color.r = 0.0 det_marker.color.g = 0.0 det_marker.color.b = 1.0 self.marker_object_in_base_pub.publish(det_marker) # print "final error is : ", final_err # print "base_target_T: ", base_object_T # computation of jacobian of the manipulator J = self.kdl_kin.jacobian(self.end_eff_current_pose) if self.used_method == "DLSM": # use of damped_least_squares as matrix inverse J_inverse = np.dot( J.T, np.linalg.inv( np.dot(J, J.T) + ((self.landa**2) * np.identity(6)))) elif self.used_method == "JPM": # use of matrix pseudo_inverse as matrix inverse J_inverse = np.linalg.pinv(J) elif self.used_method == "JTM": # use of matrix transpose as matrix inverse J_inverse = np.transpose(J) final_vel = np.dot(J_inverse, final_err2) # the dereived final speed command for the manipulator speed_cmd = list(np.array(final_vel).reshape(-1, )) # avoid huge joint velocities: joint_vel_th = [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] if speed_cmd[0] > joint_vel_th[0]: print "huge velocity for the first joint!" return elif speed_cmd[1] > joint_vel_th[1]: print "huge velocity for the second joint!" return elif speed_cmd[2] > joint_vel_th[2]: print "huge velocity for the third joint!" return elif speed_cmd[3] > joint_vel_th[3]: print "huge velocity for the fourth joint!" return elif speed_cmd[4] > joint_vel_th[4]: print "huge velocity for the fifth joint!" return elif speed_cmd[5] > joint_vel_th[5]: print "huge velocity for the sixth joint!" return elif speed_cmd[6] > joint_vel_th[6]: print "huge velocity for the seventh joint!" return # block to handle speed publisher using pose publisher intergration_time_step = 0.3 pose_change = [speed * intergration_time_step for speed in speed_cmd] final_pose = [ a + b for a, b in zip(self.end_eff_current_pose, pose_change) ] # actionlib commands self.trajectory.points[0].positions = final_pose self.trajectory.points[0].velocities = [0.0] * len(self.joint_names) self.trajectory.points[0].accelerations = [0.0] * len(self.joint_names) self.trajectory.points[0].time_from_start = rospy.Duration(1.0) # print "pose_change", pose_change # print "The manipulator is moving..." goal = FollowJointTrajectoryGoal() goal.trajectory = self.trajectory goal.goal_time_tolerance = rospy.Duration(0.0) self.manipulator_client.send_goal(goal) action_result = self.manipulator_client.wait_for_result() if action_result is True: # publishing the path of the object in "red" color final_object_position = base_object_T[0:3, 3] final_object_R = tr.identity_matrix() final_object_R[0:3, 0:3] = base_object_T[0:3, 0:3] final_object_quaternion = tr.quaternion_from_matrix(final_object_R) self.object_path.markers.append(Marker()) self.object_path.markers[-1].header.frame_id = "iiwa_base_link" self.object_path.markers[-1].header.stamp = rospy.Time.now() self.object_path.markers[-1].id = len(self.object_path.markers) self.object_path.markers[-1].type = Marker.SPHERE self.object_path.markers[ -1].pose.position.x = final_object_position[0] self.object_path.markers[ -1].pose.position.y = final_object_position[1] self.object_path.markers[ -1].pose.position.z = final_object_position[2] self.object_path.markers[ -1].pose.orientation.x = final_object_quaternion[0] self.object_path.markers[ -1].pose.orientation.y = final_object_quaternion[1] self.object_path.markers[ -1].pose.orientation.z = final_object_quaternion[2] self.object_path.markers[ -1].pose.orientation.w = final_object_quaternion[3] self.object_path.markers[-1].scale.x = 0.01 self.object_path.markers[-1].scale.y = 0.01 self.object_path.markers[-1].scale.z = 0.01 self.object_path.markers[-1].color.a = 1.0 self.object_path.markers[-1].color.r = 1.0 self.object_path.markers[-1].color.g = 0.0 self.object_path.markers[-1].color.b = 0.0 self.marker_obj_path_pub.publish(self.object_path) # publishing path of the end effector in "green" color final_ee_position = self.base_end_T[0:3, 3] final_ee_R = tr.identity_matrix() final_ee_R[0:3, 0:3] = self.base_end_T[0:3, 0:3] final_ee_quaternion = tr.quaternion_from_matrix(final_ee_R) self.ee_path.markers.append(Marker()) self.ee_path.markers[-1].header.frame_id = "iiwa_base_link" self.ee_path.markers[-1].header.stamp = rospy.Time.now() self.ee_path.markers[-1].id = len(self.ee_path.markers) self.ee_path.markers[-1].type = Marker.SPHERE self.ee_path.markers[-1].pose.position.x = final_ee_position[0] self.ee_path.markers[-1].pose.position.y = final_ee_position[1] self.ee_path.markers[-1].pose.position.z = final_ee_position[2] self.ee_path.markers[-1].pose.orientation.x = final_ee_quaternion[ 0] self.ee_path.markers[-1].pose.orientation.y = final_ee_quaternion[ 1] self.ee_path.markers[-1].pose.orientation.z = final_ee_quaternion[ 2] self.ee_path.markers[-1].pose.orientation.w = final_ee_quaternion[ 3] self.ee_path.markers[-1].scale.x = 0.01 self.ee_path.markers[-1].scale.y = 0.01 self.ee_path.markers[-1].scale.z = 0.01 self.ee_path.markers[-1].color.a = 1.0 self.ee_path.markers[-1].color.r = 0.0 self.ee_path.markers[-1].color.g = 1.0 self.ee_path.markers[-1].color.b = 0.0 self.marker_eff_path_pub.publish(self.ee_path) def set_end_cam_T(self): # translation and 180 degree rotation around z axis ??? delta_x = 0.02 delta_y = 0.175 # it used to be 0.125 delta_z = 0.03 self.end_cam_T = np.array([[-1, 0, 0, delta_x], [0, -1, 0, delta_y], [0, 0, 1, delta_z], [0, 0, 0, 1]]) # print "end_cam_T: ", self.end_cam_T def set_end_grip_T(self): # translation and ??? degree rotation arouund ? axis ??? delta_x = 0.0 delta_y = 0.0 delta_z = 0.7 self.end_grip_T = np.array([[1, 0, 0, delta_x], [0, 1, 0, delta_y], [0, 0, 1, delta_z], [0, 0, 0, 1]]) # print "end_grip_T: ", self.end_grip_T """ o_eulers = tr.euler_from_matrix(self.end_grip_T, 'rxyz') self.br.sendTransform((delta_x, delta_y, 0), tf.transformations.quaternion_from_euler(o_eulers[0], o_eulers[1], o_eulers[2]), rospy.Time.now(), "gripper", "iiwa_adapter") """ def set_grip_target_T(self): # translation and no rotation ??? delta_x = 0.0 delta_y = 0.0 delta_z = 0.05 self.grip_target_T = np.array([[1, 0, 0, delta_x], [0, 1, 0, delta_y], [0, 0, 1, delta_z], [0, 0, 0, 1]]) # print "grip_target_T: ", self.grip_target_T def set_base_end_T(self): pose = self.kdl_kin.forward(self.end_eff_current_pose) self.base_end_T = pose # print "base_end_T: ", self.base_end_T self.base_target_T = np.dot(np.dot(self.base_end_T, self.end_grip_T), self.grip_target_T) # print "base_target_T: ", self.base_target_T def set_cam_object_T(self, ball_pose): Tr = tr.translation_matrix([ball_pose[0], ball_pose[1], ball_pose[2]]) # the rotation angles are zero because the detected ball does not have any orientation xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1] Rx = tr.rotation_matrix(0, xaxis) Ry = tr.rotation_matrix(0, yaxis) Rz = tr.rotation_matrix(0, zaxis) R = tr.concatenate_matrices(Rx, Ry, Rz) T = tr.concatenate_matrices(Tr, R) return T def gripper_final_action(self): self.timer = 0 print "in gripper_final_action... " service_result = self.joint_config_client(0, 0, 0.0, 1.0) # print "sevice ", service_result pose_difference = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.22], [0, 0, 0, 1]]) final_pose_T = np.dot(self.base_target_T, pose_difference) print "final_pose_T : ", final_pose_T final_q_ik = self.kdl_kin.inverse( final_pose_T, np.array(self.end_eff_current_pose) + 0.5) # print "final_q_ik : ", np.shape(final_q_ik), type(final_q_ik) if final_q_ik is None: print "The target could not be reached!" return # actionlib commands self.trajectory.points[0].positions = final_q_ik self.trajectory.points[0].velocities = [0.0] * len(self.joint_names) self.trajectory.points[0].accelerations = [0.0] * len(self.joint_names) self.trajectory.points[0].time_from_start = rospy.Duration(1.0) goal = FollowJointTrajectoryGoal() goal.trajectory = self.trajectory goal.goal_time_tolerance = rospy.Duration(0.0) self.manipulator_client.send_goal(goal) self.manipulator_client.wait_for_result() service_result = self.joint_config_client(0, 0, 1.0, 1.0) def error_computation(self, base_object, base_target): position_err = base_object[0:3, 3] - base_target[0:3, 3] # computation of orientation_err using the skew symmetric matrix # the order for the result quaternion from tr package is x, y, z, w # no need to change the order in goal_q and ee_q cos = np.cos sin = np.sin base_object_R = tr.identity_matrix() base_object_R[0:3, 0:3] = base_object[0:3, 0:3] base_object_quaternion = tr.quaternion_from_matrix(base_object_R) goal_q = [ base_object_quaternion[0], base_object_quaternion[1], base_object_quaternion[2], base_object_quaternion[3] ] ### object tf wrt base before modification in orientation real_base_object_quaternion = tr.quaternion_from_matrix(base_object_R) object_real_q = [ real_base_object_quaternion[0], real_base_object_quaternion[1], real_base_object_quaternion[2], real_base_object_quaternion[3] ] self.br.sendTransform( (base_object[0, 3], base_object[1, 3], base_object[2, 3]), object_real_q, rospy.Time.now(), "actual_object_tf", "iiwa_base_link") # computation of orientation error using skey_symmetric matrix base_target_R = tr.identity_matrix() base_target_R[0:3, 0:3] = base_target[0:3, 0:3] base_target_quaternion = tr.quaternion_from_matrix(base_target_R) ee_q = [ base_target_quaternion[0], base_target_quaternion[1], base_target_quaternion[2], base_target_quaternion[3] ] skew_mat = np.array([[0.0, -goal_q[2], goal_q[1]], [goal_q[2], 0.0, -goal_q[0]], [-goal_q[1], goal_q[0], 0.0]]) orientation_err = np.empty((3, 1)) for i in range(3): orientation_err[i] = (ee_q[3] * goal_q[i]) - ( goal_q[3] * ee_q[i]) - (skew_mat[i, 0] * ee_q[0] + skew_mat[i, 1] * ee_q[1] + skew_mat[i, 2] * ee_q[2]) # print "orientation error: ", orientation_err final_err = np.empty((6, 1)) final_err[0] = position_err[0] final_err[1] = position_err[1] final_err[2] = position_err[2] # final_err[0] = 0 # final_err[1] = 0 # final_err[2] = 0 final_err[3] = orientation_err[0] final_err[4] = orientation_err[1] final_err[5] = orientation_err[2] # final_err[3] = 0 # final_err[4] = 0 # final_err[5] = 0 return final_err
class Autocamera: DEBUG = False # Print debug messages? def __init__(self): self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name) self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name) self.psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description') self.psm2_kin = KDLKinematics(self.psm2_robot, self.psm2_robot.links[0].name, self.psm2_robot.links[-1].name) self.zoom_level_positions = {'l1':None, 'r1':None, 'l2':None, 'r2':None, 'lm':None, 'rm':None} self.logerror("autocamera_initialized") def logerror(self, msg, debug = False): if self.DEBUG or debug: rospy.logerr(msg) def dotproduct(self, v1, v2): return sum((a*b) for a, b in zip(v1, v2)) def length(self, v): return math.sqrt(dotproduct(v, v)) def angle(self, v1, v2): return math.acos(dotproduct(v1, v2) / (length(v1) * length(v2))) def column(self, matrix, i): return [row[i] for row in matrix] def find_rotation_matrix_between_two_vectors(self, a,b): a = numpy.array(a).reshape(1,3)[0].tolist() b = numpy.array(b).reshape(1,3)[0].tolist() vector_orig = a / norm(a) vector_fin = b / norm(b) # The rotation axis (normalised). axis = cross(vector_orig, vector_fin) axis_len = norm(axis) if axis_len != 0.0: axis = axis / axis_len # Alias the axis coordinates. x = axis[0] y = axis[1] z = axis[2] # The rotation angle. angle = acos(dot(vector_orig, vector_fin)) # Trig functions (only need to do this maths once!). ca = cos(angle) sa = sin(angle) R = numpy.identity(3) # Calculate the rotation matrix elements. R[0,0] = 1.0 + (1.0 - ca)*(x**2 - 1.0) R[0,1] = -z*sa + (1.0 - ca)*x*y R[0,2] = y*sa + (1.0 - ca)*x*z R[1,0] = z*sa+(1.0 - ca)*x*y R[1,1] = 1.0 + (1.0 - ca)*(y**2 - 1.0) R[1,2] = -x*sa+(1.0 - ca)*y*z R[2,0] = -y*sa+(1.0 - ca)*x*z R[2,1] = x*sa+(1.0 - ca)*y*z R[2,2] = 1.0 + (1.0 - ca)*(z**2 - 1.0) R = numpy.matrix(R) return R def extract_positions(self, joint, arm_name, joint_kin=None): arm_name = arm_name.lower() if arm_name=="ecm": joint_kin = self.ecm_kin elif arm_name =="psm1" : joint_kin = self.psm1_kin elif arm_name =="psm2" : joint_kin = self.psm2_kin pos = [] name = [] effort = [] velocity = [] new_joint = JointState() for i in range(len(joint.position)): if joint.name[i] in joint_kin.get_joint_names(): pos.append(joint.position[i]) name.append(joint.name[i]) new_joint.name = name new_joint.position = pos return new_joint def add_marker(self, pose, name, color=[1,0,1], type=Marker.SPHERE, scale = [.02,.02,.02], points=None): vis_pub = rospy.Publisher(name, Marker, queue_size=10) marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.Time() marker.ns = "my_namespace" marker.id = 0 marker.type = type marker.action = Marker.ADD if type == Marker.LINE_LIST: for point in points: p = Point() p.x = point[0] p.y = point[1] p.z = point[2] marker.points.append(p) else: r = self.find_rotation_matrix_between_two_vectors([1,0,0], [0,0,1]) rot = pose[0:3,0:3] * r pose2 = numpy.matrix(numpy.identity(4)) pose2[0:3,0:3] = rot pose2[0:3,3] = pose[0:3,3] quat_pose = PoseConv.to_pos_quat(pose2) marker.pose.position.x = quat_pose[0][0] marker.pose.position.y = quat_pose[0][1] marker.pose.position.z = quat_pose[0][2] marker.pose.orientation.x = quat_pose[1][0] marker.pose.orientation.y = quat_pose[1][1] marker.pose.orientation.z = quat_pose[1][2] marker.pose.orientation.w = quat_pose[1][3] marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.a = .5 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] vis_pub.publish(marker) def point_towards_midpoint(self, clean_joints, psm1_pos, psm2_pos, key_hole,ecm_pose): mid_point = (psm1_pos + psm2_pos)/2 # mid_point = ecm_pose[0:3,3] - numpy.array([0,0,.01]).reshape(3,1) self.add_marker(PoseConv.to_homo_mat([mid_point, [0,0,0]]), '/marker_subscriber',color=[1,0,0], scale=[0.047/5,0.047/5,0.047/5]) self.add_marker(PoseConv.to_homo_mat([key_hole,[0,0,0]]), '/keyhole_subscriber',[0,0,1]) self.add_marker(ecm_pose, '/current_ecm_pose', [1,0,0], Marker.ARROW, scale=[.1,.005,.005]) temp = clean_joints['ecm'].position b,_ = self.ecm_kin.FK([temp[0],temp[1],.14,temp[3]]) # find the equation of the line that goes through the key_hole and the # mid_point ab_vector = (mid_point-key_hole) ecm_current_direction = b-key_hole self.add_marker(ecm_pose, '/midpoint_to_keyhole', [0,1,1], type=Marker.LINE_LIST, scale = [0.005, 0.005, 0.005], points=[b, key_hole]) self.add_marker(PoseConv.to_homo_mat([ab_vector,[0,0,0]]), '/ab_vector',[0,1,0], type=Marker.ARROW) r = self.find_rotation_matrix_between_two_vectors(ecm_current_direction, ab_vector) m = math.sqrt(ab_vector[0]**2 + ab_vector[1]**2 + ab_vector[2]**2) # ab_vector's length # insertion joint length l = math.sqrt( (ecm_pose[0,3]-key_hole[0])**2 + (ecm_pose[1,3]-key_hole[1])**2 + (ecm_pose[2,3]-key_hole[2])**2) # Equation of the line that passes through the midpoint of the tools and the key hole x = lambda t: key_hole[0] + ab_vector[0] * t y = lambda t: key_hole[1] + ab_vector[1] * t z = lambda t: key_hole[2] + ab_vector[2] * t t = l/m new_ecm_position = numpy.array([x(t), y(t), z(t)]).reshape(3,1) ecm_pose[0:3,0:3] = r* ecm_pose[0:3,0:3] ecm_pose[0:3,3] = new_ecm_position self.add_marker(ecm_pose, '/target_ecm_pose', [0,0,1], Marker.ARROW, scale=[.1,.005,.005]) output_msg = clean_joints['ecm'] try: p = self.ecm_kin.inverse(ecm_pose) except Exception as e: rospy.logerr('error') if p != None: p[3] = 0 output_msg.position = p return output_msg def zoom_fitness(self, cam_info, mid_point, inner_margin, deadzone_margin, tool_point): x = cam_info.width; y = cam_info.height # mid_point = [x/2, y/2] # if tool in inner zone if abs(tool_point[0]-mid_point[0]) <= inner_margin * x/2 and abs(tool_point[1] - mid_point[1]) < inner_margin * y/2: r = inner_margin - max([abs(tool_point[0]-mid_point[0])/(x/2),abs(tool_point[1] - mid_point[1])/(y/2)]) return r # if tool in deadzone elif abs(tool_point[0]-mid_point[0]) <= deadzone_margin * x/2 and abs(tool_point[1] - mid_point[1]) < deadzone_margin * y/2: return 0 #if tool in outer zone elif abs(tool_point[0]-mid_point[0]) <= x/2 and abs(tool_point[1] - mid_point[1]) < y/2: return -min( [ abs(min([abs(tool_point[0] - x), tool_point[0]])/(x/2) - deadzone_margin), abs(min([abs(tool_point[1] - y), tool_point[1]])/(y/2) - deadzone_margin)]) else: return -.1 # radius is a percentage of the width of the screen def zoom_fitness2(self, cam_info, mid_point, tool_point, tool_point2, radius, deadzone_radius): r = radius * cam_info.width # r is in pixels dr = deadzone_radius * cam_info.width # dr is in pixels tool_in_view = lambda tool, safespace : \ tool[0] > safespace or tool[1] > safespace or tool_point[0] < (cam_info.width-safespace) or \ tool[1] < (cam_info.height-safespace) dist = lambda a,b : norm( [i-j for i,j in zip(a,b)] ) self.logerror(dist(tool_point, tool_point2)) if dist(tool_point, mid_point) < abs(r - dr): # the tool's distance from the mid_point > r # return positive value return 0.001 # in meters elif dist(tool_point, mid_point) > abs(r + dr): # the tool's distance from the mid_point < r # return a negative value return -0.001 elif not tool_in_view(tool_point, 20) or not tool_in_view(tool_point2, 20): return -0.001 else: return 0 def unrectify_point(self, point, camera): left_ray = camera.left.projectPixelTo3dRay(point) t_vec = [0,0,0] cv2.Rodrigues( camera.left.R) def get_2d_point_from_3d_point_relative_to_world_rf(self, cam_info, TEW_inv, point): b = numpy.matrix( [ [-1, 0, 0], [0,1,0], [0,0,-1]]) # point[0:3] = b * point[0:3] P = numpy.array(cam_info.P).reshape(3,4) m = TEW_inv * point u,v,w = P * m x = u/w y = v/w Width = 640; Height = 480 x = x * .5 + .5 * Width; y = y *.5 + .5 * Height return ( x,y ) def find_zoom_level(self, msg, cam_info, clean_joints): if cam_info != None: T1W = self.psm1_kin.forward(clean_joints['psm1'].position) T2W = self.psm2_kin.forward(clean_joints['psm2'].position) TEW = self.ecm_kin.forward(clean_joints['ecm'].position) TEW_inv = numpy.linalg.inv(TEW) T1W_inv = numpy.linalg.inv(T1W) T2W_inv = numpy.linalg.inv(T2W) mid_point = (T1W[0:4,3] + T2W[0:4,3])/2 p1 = T1W[0:4,3] p2 = T2W[0:4,3] T2E = TEW_inv * T2W # ig = image_geometry.PinholeCameraModel() ig = image_geometry.StereoCameraModel() ig.fromCameraInfo(cam_info['right'], cam_info['left']) # Format in fakecam.launch: x y z yaw pitch roll [fixed-axis rotations: x(roll),y(pitch),z(yaw)] # Format for PoseConv.to_homo_mat: (x,y,z) (roll, pitch, yaw) [fixed-axis rotations: x(roll),y(pitch),z(yaw)] r = PoseConv.to_homo_mat( [ (0.0, 0.0, 0.0), (0.0, 0.0, 1.57079632679) ]) r_inv = numpy.linalg.inv(r); # r = numpy.linalg.inv(r) self.logerror( r.__str__()) # rotate_vector = lambda x: (r * numpy.array([ [x[0]], [x[1]], [x[2]], [1] ]) )[0:3,3] self.add_marker(T2W, '/left_arm', scale=[.002,.002,.002]) l1, r1 = ig.project3dToPixel( ( r_inv * TEW_inv * T1W )[0:3,3]) # tool1 left and right pixel positions l2, r2 = ig.project3dToPixel( ( r_inv * TEW_inv * T2W )[0:3,3]) # tool2 left and right pixel positions lm, rm = ig.project3dToPixel( ( r_inv * TEW_inv * mid_point)[0:3,0]) # midpoint left and right pixel positions # add_100 = lambda x : (x[0] *.5 + cam_info.width/2, x[1]) # l1 = add_100(l1) # l2 = add_100(l2) # lm = add_100(lm) self.zoom_level_positions = {'l1':l1, 'r1':r1, 'l2':l2, 'r2':r2, 'lm':lm, 'rm':rm} test1_l, test1_r = ig.project3dToPixel( [1,0,0]) test2_l, test2_r = ig.project3dToPixel( [0,0,1]) self.logerror('\ntest1_l = ' + test1_l.__str__() + '\ntest2_l = ' + test2_l.__str__() ) # logerror('xm=%f,'%lm[0] + 'ym=%f'%lm[1]) msg.position[3] = 0 # zoom_percentage = zoom_fitness(cam_info=cam_info, mid_point=[xm, ym], inner_margin=.20, # deadzone_margin= .70, tool_point= [x1,y1]) zoom_percentage = self.zoom_fitness2(cam_info['left'], mid_point=lm, tool_point=l1, tool_point2=l2, radius=.1, deadzone_radius=.01) msg.position[2] = msg.position[2] + zoom_percentage if msg.position[2] < 0 : # minimum 0 msg.position[2] = 0.00 elif msg.position[2] > .21: # maximum .23 msg.position[2] = .21 return msg def compute_viewangle(self, joint, cam_info): kinematics = lambda name: self.psm1_kin if name == 'psm1' else self.psm2_kin if name == 'psm2' else self.ecm_kin clean_joints = {} try: joint_names = joint.keys() for j in joint_names: clean_joints[j] = self.extract_positions(joint[j], j, kinematics(j)) key_hole, _ = self.ecm_kin.FK([0,0,0,0]) # The position of the keyhole, is the end-effector's psm1_pos,_ = self.psm1_kin.FK(clean_joints['psm1'].position) psm2_pos,_ = self.psm2_kin.FK(clean_joints['psm2'].position) psm1_pose = self.psm1_kin.forward(clean_joints['psm1'].position) ecm_pose = self.ecm_kin.forward(clean_joints['ecm'].position) except Exception as e: # rospy.logerr(e.message) output_msg = joint['ecm'] # output_msg.name = ['outer_yaw', 'outer_pitch', 'insertion', 'outer_roll'] # output_msg.position = [joint['ecm'].position[x] for x in [0,1,5,6]] return output_msg output_msg = clean_joints['ecm'] gripper= max( [ abs(joint['psm1'].position[-1]), abs(joint['psm2'].position[-1])] ) < math.pi/8 # rospy.logerr('psm1 gripper = ' + joint['psm1'].position[-1].__str__() + 'gripper = ' + gripper.__str__()) if gripper == gripper or gripper != gripper: # luke was here output_msg = self.point_towards_midpoint(clean_joints, psm1_pos, psm2_pos, key_hole, ecm_pose) output_msg = self.find_zoom_level(output_msg, cam_info, clean_joints) pass if len(output_msg.name) > 4: output_msg.name = ['outer_yaw', 'outer_pitch', 'insertion', 'outer_roll'] if len(output_msg.position) >= 7: output_msg.position = [output_msg.position[x] for x in [0,1,5,6]] self.logerror(output_msg.__str__()) return output_msg
class BallWatcher(object): def __init__(self): rospy.loginfo("Creating BallWatcher class") self.print_help() self.objDetector = Obj3DDetector() # flags and vars # for ball dropping position self.kinect_calibrate_flag = False self.running_flag = False self.start_calc_flag = False self.ball_marker = sawyer_mk.MarkerDrawer("/base", "ball", 500) self.drop_point = Point() self.drop_point_arr = [] self.pos_rec_list = np.array([]) self.drop_point_marker = sawyer_mk.MarkerDrawer( "/base", "dropping_ball", 500) self.ik_cont = IKController() self.robot = URDF.from_parameter_server() self.kin = KDLKinematics(self.robot, BASE, EE_FRAME) self.limb_interface = intera_interface.Limb() self.pos_rec = [ PointStamped() for i in range(2) ] # array 1x2 with each cell storing a PointStamped with cartesian position of the ball in the past two frames self.old_pos_rec = [PointStamped() for i in range(2)] self.last_tf_time = rospy.Time() self.tf_listener = tf.TransformListener() self.tf_bc = tf.TransformBroadcaster() self.tf_listener.waitForTransform("base", "camera_link", rospy.Time(), rospy.Duration(0.5)) self.T_sc_p, self.T_sc_q = self.tf_listener.lookupTransform( "base", "camera_link", rospy.Time()) self.T_sc = tr.euler_matrix(*tr.euler_from_quaternion(self.T_sc_q)) self.T_sc[0:3, -1] = self.T_sc_p # kbhit instance self.kb = kbhit.KBHit() rospy.on_shutdown(self.kb.set_normal_term) # publishers and timers self.kb_timer = rospy.Timer(rospy.Duration(0.1), self.keycb) self.tf_timer = rospy.Timer(rospy.Duration(0.01), self.tf_update_cb) self.final_x_total = 0 self.final_y_total = 0 def check_start_throw(self): # check if the ball is being thrown, by comparing between z of the first and second frame z_past = self.pos_rec[0].point.z z_present = self.pos_rec[1].point.z if (z_present - z_past > 0.05): self.start_calc_flag = True def check_stop_throw(self): # check if the ball reach the end of trajectory, if so, raise down the start_calc_flag x_past = self.pos_rec[0].point.x x_present = self.pos_rec[1].point.x if (x_present - x_past > 0): self.start_calc_flag = False self.pos_rec_list = np.array([]) # print "test" def roll_mat(self, mat): row_num = len(mat) for i in range(0, row_num - 1): mat[i] = mat[i + 1] return def tf_update_cb(self, tdat): # update ball position in real time # Filter ball outside x = 0 - 3.0m relative to base out self.p_cb = self.objDetector.p_ball_to_cam # print self.p_cb if (self.p_cb == []): # print "Please wait, the system is detecting the ball" return # try: # self.tf_listener.waitForTransform(TARGET_FRAME, SOURCE_FRAME, rospy.Time(), rospy.Duration(0.5)) # except tf.Exception: # rospy.loginfo("No frame of /ball from /base received, stop calculation. self.start_calc_flag = False") # p, q = self.tf_listener.lookupTransform(TARGET_FRAME, SOURCE_FRAME, rospy.Time()) # change to subscribe lookuptf directly and multiply directly # P_cb = copy.deepcopy(self.objDetector.p_ball_to_cam) P_cb = np.array([ self.objDetector.p_ball_to_cam[0], self.objDetector.p_ball_to_cam[1], self.objDetector.p_ball_to_cam[2], self.objDetector.p_ball_to_cam[3] ]) timestamp = P_cb[3] P_cb[3] = 1 P_cb = P_cb.reshape((4, 1)) p_sb = np.dot(self.T_sc, P_cb) self.tf_bc.sendTransform((p_sb[0][0], p_sb[1][0], p_sb[2][0]), [0, 0, 0, 1], rospy.Time.now(), "ball", "base") pos = PointStamped() pos.header.stamp = timestamp # pos.point.x = p[0] # pos.point.y = p[1] # pos.point.z = p[2] # print "1: ", p_sb[0][0] # print "2: ", p_sb[1][0] # print "3: ", p_sb[2][0] pos.point.x = p_sb[0][0] pos.point.y = p_sb[1][0] pos.point.z = p_sb[2][0] # filter repeated received tf out if (self.pos_rec[-1].header.stamp != pos.header.stamp) and ( self.pos_rec[-1].point.x != pos.point.x) and (pos.point.x < 3.5) and ( pos.point.x - self.pos_rec[-1].point.x < 1.2): self.roll_mat(self.pos_rec) self.pos_rec[-1] = pos # choose only a non-repeated pos_rec by comparing between the timestamped of the present and past pos_rec if (self.last_tf_time != self.pos_rec[0].header.stamp): # If running_flag is True, start detecting if self.running_flag: # check if the ball is being thrown yet if not self.start_calc_flag: self.check_start_throw() self.ball_marker.draw_spheres([0, 0.7, 0, 1], [0.03, 0.03, 0.03], self.pos_rec[0].point) self.ball_marker.draw_line_strips([5.7, 1, 4.7, 1], [0.01, 0, 0], self.pos_rec[0].point, self.pos_rec[1].point) if self.start_calc_flag: self.check_stop_throw() if not self.start_calc_flag: return # draw markers self.ball_marker.draw_spheres([0.7, 0, 0, 1], [0.03, 0.03, 0.03], self.pos_rec[0].point) self.ball_marker.draw_line_strips([1, 0.27, 0.27, 1], [0.01, 0, 0], self.pos_rec[0].point, self.pos_rec[1].point) self.ball_marker.draw_numtxts([1, 1, 1, 1], 0.03, self.pos_rec[0].point, 0.03) # calculate the dropping position based on 2 points print "##########" print "pos_rec_listB4: ", self.pos_rec_list self.pos_rec_list = np.append(self.pos_rec_list, self.pos_rec[0]) # self.pos_rec_list = (self.pos_rec_list).append(self.pos_rec[0]) print "pos_rec_list: ", self.pos_rec_list print "##########" self.drop_point = sawyer_calc.opt_min_proj_calc( self.pos_rec_list, Z_CENTER) # average drop point input_posestamped = PoseStamped() input_posestamped.pose.position = self.drop_point input_posestamped.pose.orientation = Quaternion( 0.0392407571798, 0.664506667783, -0.0505321422468, 0.744538483926) if self.pos_rec_list.shape[0] >= 4: self.ik_cont.running_flag = True self.ik_cont.set_goal_from_pose(input_posestamped) self.drop_point_marker.draw_spheres([0, 0, 0.7, 1], [0.03, 0.03, 0.03], self.drop_point) self.drop_point_marker.draw_numtxts([1, 1, 1, 1], 0.03, self.drop_point, 0.03) self.last_tf_time = self.pos_rec[0].header.stamp def keycb(self, tdat): # check if there was a key pressed, and if so, check it's value and # toggle flag: if self.kb.kbhit(): c = self.kb.getch() if ord(c) == 27: rospy.signal_shutdown("Escape pressed!") else: print c if c == 's': rospy.loginfo( "You pressed 's', Program starts. Sawyer is waiting for the ball to be thrown." ) self.running_flag = not self.running_flag if not self.running_flag: self.ball_marker.delete_all_mks() self.pos_rec_list = [] self.ik_cont.running_flag = False elif c == 'c': rospy.loginfo( "You pressed 'c', Start calibration\nrunning_flag = False") self.running_flag = False self.kinect_pos_calib() elif c == 'h': rospy.loginfo( "You pressed 'h', Go to home position\nrunning_flag = False" ) self.running_flag = False self.home_pos() elif c == 'p': rospy.loginfo("You pressed 'h', Plotting") self.plot_flag = True else: self.print_help() self.kb.flush() return def kinect_pos_calib(self): # if not self.kinect_calibrate_flag: # input x,y,z and x,y,z p = [X_KINECT_CALIBR, Y_KINECT_CALIBR, Z_KINECT_CALIBR] self.kinect_calibrate_flag = True rospy.loginfo( "Press C again for calibrate the height of the camera") else: p = [X_KINECT_CALIBR_2, Y_KINECT_CALIBR, Z_KINECT_CALIBR] self.kinect_calibrate_flag = False G = tr.euler_matrix(ROLL_KINECT_CALIBR, PITCH_KINECT_CALIBR, YAW_KINECT_CALIBR) for i in range(3): G[i][3] = p[i] q_seed = self.kin.random_joint_angles() j_names = self.kin.get_joint_names() q = self.kin.inverse(G, q_seed) while (q is None): q_seed = self.kin.random_joint_angles() q = self.kin.inverse(G, q_seed) j_req = dict(zip(j_names, q)) self.limb_interface.move_to_joint_positions(j_req) def home_pos(self): self.limb_interface.move_to_joint_positions({ 'head_pan': 0.379125, 'right_j0': -0.020025390625, 'right_j1': 0.8227529296875, 'right_j2': -2.0955126953125, 'right_j3': 2.172509765625, 'right_j4': 0.7021171875, 'right_j5': -1.5003603515625, 'right_j6': -2.204990234375 }) def print_help(self): help_string = \ """ 's' ~ Start the program 'c' ~ Calibration the kinect position 'h' ~ Move to home position 'p' ~ Plot the trajectory of each Cartesian coordinate 'ESC' ~ Quit """ print help_string return
def __init__(self, hebi_group_name, hebi_mapping, leg_base_links, leg_end_links): rospy.loginfo("Creating Hexapod instance...") hebi_families = [] hebi_names = [] for leg in hebi_mapping: for actuator in leg: family, name = actuator.split('/') hebi_families.append(family) hebi_names.append(name) rospy.loginfo(" hebi_group_name: %s", hebi_group_name) rospy.loginfo(" hebi_families: %s", hebi_families) rospy.loginfo(" hebi_names: %s", hebi_names) self.hebi_mapping = hebi_mapping self.hebi_mapping_flat = self._flatten(self.hebi_mapping) # jt information populated by self._feedback_cb self._current_jt_pos = {} self._current_jt_vel = {} self._current_jt_eff = {} self._joint_state_pub = None self._hold_leg_list = [False, False, False, False, False, False] self._hold_leg_positions = self._get_list_of_lists() # Create a service client to create a group set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names', AddGroupFromNamesSrv) # Topic to receive feedback from a group self.hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state" rospy.loginfo(" hebi_group_feedback_topic: %s", "/hebiros/" + hebi_group_name + "/feedback/joint_state") # Topic to send commands to a group self.hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state" rospy.loginfo(" hebi_group_command_topic: %s", "/hebiros/" + hebi_group_name + "/command/joint_state") # Call the /hebiros/add_group_from_names service to create a group rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...", '/hebiros/add_group_from_names') rospy.wait_for_service('/hebiros/add_group_from_names' ) # block until service server starts rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.") set_hebi_group(hebi_group_name, hebi_names, hebi_families) # Create a service client to set group settings change_group_settings = rospy.ServiceProxy( "/hebiros/" + hebi_group_name + "/send_command_with_acknowledgement", SendCommandWithAcknowledgementSrv) rospy.loginfo( " Waiting for SendCommandWithAcknowledgementSrv at %s ...", "/hebiros/" + hebi_group_name + "/send_command_with_acknowledgement") rospy.wait_for_service("/hebiros/" + hebi_group_name + "/send_command_with_acknowledgement" ) # block until service server starts cmd_msg = CommandMsg() cmd_msg.name = self.hebi_mapping_flat cmd_msg.settings.name = self.hebi_mapping_flat cmd_msg.settings.position_gains.name = self.hebi_mapping_flat cmd_msg.settings.position_gains.kp = [5, 8, 2] * LEGS cmd_msg.settings.position_gains.ki = [0.001] * LEGS * ACTUATORS_PER_LEG cmd_msg.settings.position_gains.kd = [0] * LEGS * ACTUATORS_PER_LEG cmd_msg.settings.position_gains.i_clamp = [ 0.25 ] * LEGS * ACTUATORS_PER_LEG # TODO: Tune this. Setting it low for testing w/o restarting Gazebo change_group_settings(cmd_msg) # Feedback/Command self.fbk_sub = rospy.Subscriber(self.hebi_group_feedback_topic, JointState, self._feedback_cb) self.cmd_pub = rospy.Publisher(self.hebi_group_command_topic, JointState, queue_size=1) self._hold_position = False self._hold_joint_states = {} # TrajectoryAction client self.trajectory_action_client = SimpleActionClient( "/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction) rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...", "/hebiros/" + hebi_group_name + "/trajectory") self.trajectory_action_client.wait_for_server( ) # block until action server starts rospy.loginfo(" TrajectoryActionServer AVAILABLE.") # Twist Subscriber self._cmd_vel_sub = rospy.Subscriber("/hexapod/cmd_vel/", Twist, self._cmd_vel_cb) self.last_vel_cmd = None self.linear_displacement_limit = 0.075 # m self.angular_displacement_limit = 0.65 # rad # Check ROS Parameter server for robot_description URDF urdf_str = "" urdf_loaded = False while not rospy.is_shutdown() and not urdf_loaded: if rospy.has_param('/robot_description'): urdf_str = rospy.get_param('/robot_description') urdf_loaded = True rospy.loginfo( "Pulled /robot_description from parameter server.") else: rospy.sleep(0.01) # sleep for 10 ms of ROS time # pykdl_utils setup self.robot_urdf = URDF.from_xml_string(urdf_str) self.kdl_fk_base_to_leg_base = [ KDLKinematics(self.robot_urdf, 'base_link', base_link) for base_link in leg_base_links ] self.kdl_fk_leg_base_to_eff = [ KDLKinematics(self.robot_urdf, base_link, end_link) for base_link, end_link in zip(leg_base_links, leg_end_links) ] # trac-ik setup self.trac_ik_leg_base_to_end = [ IK( base_link, end_link, urdf_string=urdf_str, timeout=0.01, # TODO: Tune me epsilon=1e-4, solve_type="Distance") for base_link, end_link in zip(leg_base_links, leg_end_links) ] self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01] self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0 ] # NOTE: This implements position-only IK # Wait for connections to be set up rospy.loginfo("Wait for ROS connections to be set up...") while not rospy.is_shutdown() and len(self._current_jt_pos) < len( self.hebi_mapping_flat): rospy.sleep(0.1) # Set up joint state publisher self._joint_state_pub = rospy.Publisher('/joint_states', JointState, queue_size=1) self._loop_rate = rospy.Rate(20) # leg joint home pos Hip, Knee, Ankle self.leg_jt_home_pos = [ [0.0, +0.26, -1.57], # Leg 1 [0.0, -0.26, +1.57], # Leg 2 [0.0, +0.26, -1.57], # Leg 3 [0.0, -0.26, +1.57], # Leg 4 [0.0, +0.26, -1.57], # Leg 5 [0.0, -0.26, +1.57] ] # Leg 6 # leg end-effector home position self.leg_eff_home_pos = self._get_leg_base_to_eff_fk( self.leg_jt_home_pos) # leg step height relative to leg base link self.leg_eff_step_height = [[]] * LEGS # relative to leg base for i, fk_solver in enumerate(self.kdl_fk_base_to_leg_base): base_to_leg_base_rot = fk_solver.forward([])[:3, :3] step_ht_chassis = np.array([0, 0, STEP_HEIGHT]) step_ht_leg_base = np.dot(base_to_leg_base_rot, step_ht_chassis) self.leg_eff_step_height[i] = step_ht_leg_base.tolist()[0] self._odd_starts = True rospy.loginfo("Done creating Hexapod instance...")
class Robot(object): def __init__(self, sim=True): f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r') robot = URDF.from_xml_string(f.read()) # parsed URDF # robot = URDF.from_parameter_server() self.kin = KDLKinematics(robot, 'base', 'mconn') # x, y, z, yaw, pitch self.cart_from_matrix = lambda T: np.array([ T[0, 3], T[1, 3], T[2, 3], math.atan2(T[1, 0], T[0, 0]), -math.asin(T[2, 0])]) self.home = np.array([0.05, 0, 0.2, 0, 0]) self.q = np.array([0, 0, 0, 0, 0]) self.dt = 0.05 self.sim = sim def publish(eventStop): # for arbotix pub1 = rospy.Publisher("q1/command", Float64, queue_size=5) pub2 = rospy.Publisher("q2/command", Float64, queue_size=5) pub3 = rospy.Publisher("q3/command", Float64, queue_size=5) pub4 = rospy.Publisher("q4/command", Float64, queue_size=5) pub5 = rospy.Publisher("q5/command", Float64, queue_size=5) # for visualization jointPub = rospy.Publisher( "/joint_states", JointState, queue_size=5) jmsg = JointState() jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5'] while not rospy.is_shutdown() and not eventStop.is_set(): jmsg.header.stamp = rospy.Time.now() jmsg.position = self.q if self.sim: jointPub.publish(jmsg) pub1.publish(self.q[0]) pub2.publish(self.q[1]) pub3.publish(self.q[2]) pub4.publish(self.q[3]) pub5.publish(self.q[4]) eventStop.wait(self.dt) rospy.init_node("robot") eventStop = threading.Event() threadJPub = threading.Thread(target=publish, args=(eventStop,)) threadJPub.daemon = True threadJPub.start() def forward(self, q=None): if q is None: q = self.q return(self.kin.forward(q)) def cart(self, q=None): if q is None: q = self.q return(self.cart_from_matrix(self.forward(q))) def jacobian(self, q=None): if q is None: q = self.q eps = 1e-3 s0 = np.array(self.cart(q)) dof = len(self.q) J = np.array(dof * [dof * [0.0]]) for k in range(0, dof): dq = np.zeros(dof) dq[k] = eps J[:, k] = (self.cart(q + dq) - s0) / eps return J def move(self, s1, speed=1): def plan(s1): # return tuples of joint coordinates to be moved at every self.dt # first calculate goal_joint def error_function(q): return ( np.array(self.cart(q)) - np.array(s1)) x, _, ler, message = scipy.optimize.fsolve( error_function, 0*self.q, full_output=True) if not ler == 1: # print "No solution found. Target: ", goal_space # res = minimize(lambda q: np.linalg.norm(error_function(q)), self.q*0) # x = res.x # if not res.success: # raise Exception('No IK solution found') raise Exception('No IK solution found') q1 = (x + math.pi) % (2 * math.pi) - math.pi # q1 = x # break trajectory to intermediate points q0 = self.q s0 = self.cart(q0) t1 = np.max(np.abs(q1 - q0)) / speed time = np.arange(0, t1, self.dt) + self.dt dof = len(self.q) qq = np.array([np.interp(time, [0, t1], [q0[k], q1[k]]) for k in range(dof)]).transpose() return qq traj = plan(s1) rate = rospy.Rate(1.0 / self.dt) for joints in traj: self.q = joints rate.sleep() def move2(self, s1): speed = 1.5 TOL = 1e-8 MAX_VEL = 0.2 t0 = rospy.Time.now() while rospy.Time.now() - t0 < rospy.Duration(5): ds = s1 - self.cart() dev = np.linalg.norm(ds) if dev < TOL: print("Success. Deviation: ") print(dev) return J = self.jacobian() dq = np.linalg.solve(J, ds) if max(abs(dq)) > MAX_VEL: dq = dq * MAX_VEL / max(abs(dq)) alfa = speed * self.dt * (rospy.Time.now() - t0).to_sec()**1.5 self.q = self.q + dq * alfa rospy.sleep(self.dt) print("Failure. Deviation: ") print(dev) def reset(self): self.move(self.cart(self.q*0))
def __init__(self, base_link, end_link, planning_group, world="/world", listener=None, traj_step_t=0.1, max_acc=1, max_vel=1, max_goal_diff = 0.02, goal_rotation_weight = 0.01, max_q_diff = 1e-6, start_js_cb=True, base_steps=10, steps_per_meter=100): self.world = world self.base_link = base_link self.end_link = end_link self.planning_group = planning_group self.base_steps = base_steps self.steps_per_meter = steps_per_meter self.MAX_ACC = max_acc self.MAX_VEL = max_vel self.traj_step_t = traj_step_t self.max_goal_diff = max_goal_diff self.max_q_diff = max_q_diff self.goal_rotation_weight = goal_rotation_weight self.at_goal = True self.near_goal = True self.moving = False self.q0 = [0,0,0,0,0,0,0] self.old_q0 = [0,0,0,0,0,0,0] self.cur_stamp = 0 self.teach_mode = rospy.Service('/costar/SetTeachMode',SetTeachMode,self.set_teach_mode_call) self.servo_mode = rospy.Service('/costar/SetServoMode',SetServoMode,self.set_servo_mode_call) self.shutdown = rospy.Service('/costar/ShutdownArm',EmptyService,self.shutdown_arm_call) self.servo = rospy.Service('/costar/ServoToPose',ServoToPose,self.servo_to_pose_call) self.plan = rospy.Service('/costar/PlanToPose',ServoToPose,self.plan_to_pose_call) self.smartmove = rospy.Service('/costar/SmartMove',SmartMove,self.smart_move_call) self.js_servo = rospy.Service('/costar/ServoToJointState',ServoToJointState,self.servo_to_joints_call) self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd',JointTrajectoryPoint,queue_size=1000) self.get_waypoints_srv = GetWaypointsService(world=world,service=False) self.driver_status = 'IDLE' self.status_publisher = rospy.Publisher('/costar/DriverStatus',String,queue_size=1000) self.robot = URDF.from_parameter_server() if start_js_cb: self.js_subscriber = rospy.Subscriber('joint_states',JointState,self.js_cb) self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if listener is None: self.listener = tf.TransformListener() else: self.listener = listener #print self.tree.getNrOfSegments() #print self.chain.getNrOfJoints() self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) self.display_pub = rospy.Publisher('costar/display_trajectory',DisplayTrajectory,queue_size=1000) #self.set_goal(self.q0) self.goal = None self.ee_pose = None self.planner = SimplePlanning(self.robot,base_link,end_link,self.planning_group)
class OmniMiniProj: def __init__(self): #Create a tf listener self.tfListener = tf.TransformListener() #tf.TransfromListener is a subclass that subscribes to the "/tf" message topic and adds transform #data to the tf data structure. As a result the object is up to date with all current transforms #Create a publisher to a new topic name called "stylus_to_base_transf with a message type Twist" self.Tsb = rospy.Publisher('stylus_to_base_transf',Twist) #Find the omni from the parameter server self.omni = URDF.from_parameter_server() #Note: A node that initializes and runs the Phantom Omni has to be running in the background for # from_parameter_server to to find the parameter. # Older versions of URDF label the function "load_from_parameter_server" #Subscribe to the "omni1_joint_states" topic, which provides the joint states measured by the omni in a # ROS message of the type sensor_msgs/JointState. Every time a message is published to that topic run the #callback function self.get_joint_states, which is defined below. self.joint_state_sub = rospy.Subscriber("omni1_joint_states", JointState, self.get_joint_states) #OmniMiniProjTimers #Run a timer to manipulate the class structure as needed. The timer's #callback function "timer_callback" then calls the desired functions self.timer1 = rospy.Timer(rospy.Duration(0.01), self.timer_callback) #Create a separate slower timer on a lower frequency for a comfortable print out self.print_timer = rospy.Timer(rospy.Duration(1), self.print_output) return #The timer_callback runs every time timer1 triggers. In this example, it merely calls other functions. Generally, #it may include further calculations, just like any regular function. def timer_callback(self, data): self.tf_base_to_stylus() return #get_joint_states extracts the joint agles from the JointState message (angles, agular velocity, ...), which is #provided by the 'joint_state_sub' subscriber. def get_joint_states(self,data): try: q_sensors = data.position except rospy.ROSInterruptException: self.q_sensors = None pass if q_sensors != None: self.kdl_kinematics(q_sensors) return #tf_base_to_stylus uses tf to look up the transfrom from the frame located at the root of the Omni's URDF (/base) to the frame #at the end of the URDF (/stylus). def tf_base_to_stylus (self): try: (self.transl,self.quat) = self.tfListener.lookupTransform('base','stylus',rospy.Time(0)) #lookupTransrom is a method which returns the transfrom between two coordinate frames. #lookupTransfrom returns a translation and a quaternion self.rot= euler_from_quaternion(self.quat) #Get euler angles from the quaternion self.tf_SE3 = compose_matrix(angles=self.rot,translate=self.transl) #Store the transformation in a format compatible with gemoetr_msgs/Twist self.transf = Twist(Vector3(self.transl[0],self.transl[1],self.transl[2]),(Vector3(self.rot[0],self.rot[1],self.rot[2]))) #Publish the transformation self.Tsb.publish(self.transf) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): self.tf_SE3 = None pass #If exceptions occur, skip trying to lookup the transform. #It is encouraged to publish a logging message to rosout with rospy. #i.e: #rospy.logerr("Could not transform from %s to %s,"base","stylus") return def kdl_kinematics (self,data): self.q_sensors = data self.tree = kdl_tree_from_urdf_model(self.omni) # create a kdl tree from omni URDF model self.omni_kin = KDLKinematics(self.omni, "base", "stylus") # create instance that captures the kinematics of the robot arm #Forward Kinematics self.pose_stylus = self.omni_kin.forward(data) #compute the forward kinematics from the sensor joint angle position using the kinematics from the kdl tree #Inverse Kinematics self.q_guess = numpy.array(data)+0.2 #make an initial guess for your joint angles by deviating all the sensor joint angles by 0.2 self.q_ik = self.omni_kin.inverse(self.pose_stylus, self.q_guess) #using the position from the forward kinematics 'pose_stylus' and the offset initial guess, compute #the desired joint angles for that position. self.delta_q = self.q_ik-data def print_output (self,data): if self.tf_SE3 != None and self.q_sensors !=None: print "Base to stylus using tf:","\n", self.tf_SE3, "\n" print "base to stylus using KDL forward kinematics", "\n" , self.pose_stylus, "\n" print "transformations.is.same_transform tf vs KDL forward kin:", "\n", is_same_transform(self.tf_SE3,self.pose_stylus), "\n" print "Joint angles from sensors q_sensors:","\n" , self.q_sensors, "\n" print "Joint state angles from KDL inverse kinematics q_ik", "\n" , self.q_ik , "\n" print "The difference delta_q between q_sensors and q_ik", "\n" , self.delta_q , "\n\n\n\n\n\n\n\n\n" return
class CostarArm(object): def __init__(self, base_link, end_link, planning_group, world="/world", listener=None, traj_step_t=0.1, max_acc=1, max_vel=1, max_goal_diff = 0.02, goal_rotation_weight = 0.01, max_q_diff = 1e-6, start_js_cb=True, base_steps=10, steps_per_meter=100): self.world = world self.base_link = base_link self.end_link = end_link self.planning_group = planning_group self.base_steps = base_steps self.steps_per_meter = steps_per_meter self.MAX_ACC = max_acc self.MAX_VEL = max_vel self.traj_step_t = traj_step_t self.max_goal_diff = max_goal_diff self.max_q_diff = max_q_diff self.goal_rotation_weight = goal_rotation_weight self.at_goal = True self.near_goal = True self.moving = False self.q0 = [0,0,0,0,0,0,0] self.old_q0 = [0,0,0,0,0,0,0] self.cur_stamp = 0 self.teach_mode = rospy.Service('/costar/SetTeachMode',SetTeachMode,self.set_teach_mode_call) self.servo_mode = rospy.Service('/costar/SetServoMode',SetServoMode,self.set_servo_mode_call) self.shutdown = rospy.Service('/costar/ShutdownArm',EmptyService,self.shutdown_arm_call) self.servo = rospy.Service('/costar/ServoToPose',ServoToPose,self.servo_to_pose_call) self.plan = rospy.Service('/costar/PlanToPose',ServoToPose,self.plan_to_pose_call) self.smartmove = rospy.Service('/costar/SmartMove',SmartMove,self.smart_move_call) self.js_servo = rospy.Service('/costar/ServoToJointState',ServoToJointState,self.servo_to_joints_call) self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd',JointTrajectoryPoint,queue_size=1000) self.get_waypoints_srv = GetWaypointsService(world=world,service=False) self.driver_status = 'IDLE' self.status_publisher = rospy.Publisher('/costar/DriverStatus',String,queue_size=1000) self.robot = URDF.from_parameter_server() if start_js_cb: self.js_subscriber = rospy.Subscriber('joint_states',JointState,self.js_cb) self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if listener is None: self.listener = tf.TransformListener() else: self.listener = listener #print self.tree.getNrOfSegments() #print self.chain.getNrOfJoints() self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) self.display_pub = rospy.Publisher('costar/display_trajectory',DisplayTrajectory,queue_size=1000) #self.set_goal(self.q0) self.goal = None self.ee_pose = None self.planner = SimplePlanning(self.robot,base_link,end_link,self.planning_group) ''' js_cb listen to robot joint state information ''' def js_cb(self,msg): self.old_q0 = self.q0 self.q0 = np.array(msg.position) self.update_position() ''' update current position information ''' def update_position(self): self.ee_pose = pm.fromMatrix(self.kdl_kin.forward(self.q0)) if self.goal is not None: #goal_diff = np.abs(self.goal - self.q0).sum() / self.q0.shape[0] cart_diff = (self.ee_pose.p - self.goal.p).Norm() rot_diff = self.goal_rotation_weight * (pm.Vector(*self.ee_pose.M.GetRPY()) - pm.Vector(*self.goal.M.GetRPY())).Norm() goal_diff = cart_diff + rot_diff if goal_diff < self.max_goal_diff: self.at_goal = True if goal_diff < 10*self.max_goal_diff: self.near_goal = True q_diff = np.abs(self.old_q0 - self.q0).sum() if q_diff < self.max_q_diff: self.moving = False else: self.moving = True def check_req_speed_params(self,req): if req.accel > self.MAX_ACC: acceleration = self.MAX_ACC else: acceleration = req.accel if req.vel > self.MAX_VEL: velocity = self.MAX_VEL else: velocity = req.vel return (acceleration, velocity) ''' Find any valid object that meets the requirements. Find a cartesian path or possibly longer path through joint space. ''' def smart_move_call(self,req): if False and not self.driver_status == 'SERVO': rospy.logerr('DRIVER -- Not in servo mode!') return 'FAILURE - not in servo mode' (acceleration, velocity) = self.check_req_speed_params(req) (poses,names) = self.get_waypoints_srv.get_waypoints( req.obj_class, # object class to move to req.predicates, # predicates to match [req.pose], # offset/transform from each member of the class ["tmp"] # placeholder name ) print req.obj_class print req.predicates print names print poses if not poses is None: msg = 'FAILURE - no valid objects found!' qs = [] dists = [] for (pose,name) in zip(poses,names): # figure out which tf frame we care about tf_path = name.split('/') tf_frame = '' for part in tf_path: if len(part) > 0: tf_frame = part break if len(tf_frame) == 0: continue # try to move to the pose until one succeeds T_base_world = pm.fromTf(self.listener.lookupTransform(self.world,self.base_link,rospy.Time(0))) T = T_base_world.Inverse()*pm.fromMsg(pose) # Check acceleration and velocity limits # Send command pt = JointTrajectoryPoint() #q = self.planner.ik(T, self.q0) traj = self.planner.getCartesianMove(T,self.q0,self.base_steps,self.steps_per_meter) #if len(traj.points) == 0: # print T # (code,res) = self.planner.getPlan(pm.toMsg(T),self.q0) # find a non-local movement # traj = res.planned_trajectory.joint_trajectory print "Considering object with name = %s"%tf_frame if len(traj.points) > 0: qs.append(traj) dists.append((traj.points[-1].positions - self.q0).sum()) else: rospy.logwarn('SIMPLE DRIVER -- IK failed for %s'%name) if len(qs) == 0: msg = 'FAILURE - no joint configurations found!' possible_goals = zip(dists,qs) possible_goals.sort() print "POSSIBLE GOALS" print possible_goals (dist,traj) = possible_goals[0] rospy.logwarn("Trying to move to frame at distance %f"%(dist)) msg = self.send_trajectory(traj,acceleration,velocity,cartesian=True) return msg else: msg = 'FAILURE - no match to predicate moves' return msg def set_goal(self,q): self.at_goal = False self.near_goal = False self.goal = pm.fromMatrix(self.kdl_kin.forward(q)) ''' Definitely do a planned motion. ''' def plan_to_pose_call(self,req): #rospy.loginfo('Recieved servo to pose request') #print req if self.driver_status == 'SERVO': T = pm.fromMsg(req.target) # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) # Send command pt = JointTrajectoryPoint() traj = self.planner.getCartesianMove(T,self.q0,self.base_steps,self.steps_per_meter) if len(traj.points) > 0: (code,res) = self.planner.getPlan(req.target,traj.points[-1].positions) else: (code,res) = self.planner.getPlan(req.target,self.q0) #pt.positions = q if code > 0: self.at_goal = True return 'SUCCESS - at goal' if (not res is None) and len(res.planned_trajectory.joint_trajectory.points) > 0: disp = DisplayTrajectory() disp.trajectory.append(res.planned_trajectory) disp.trajectory_start = res.trajectory_start self.display_pub.publish(disp) traj = res.planned_trajectory.joint_trajectory return self.send_trajectory(traj,acceleration,velocity,cartesian=True) else: rospy.logerr(res) rospy.logerr('DRIVER -- PLANNING failed') return 'FAILURE - not in servo mode' else: rospy.logerr('DRIVER -- not in servo mode!') return 'FAILURE - not in servo mode' ''' Send a whole joint trajectory message to a robot... that is listening to individual joint states. ''' def send_trajectory(self,traj,acceleration=0.5,velocity=0.5,cartesian=False): rospy.logerr("Function 'send_trajectory' not implemented for base class!") return "FAILURE - running base class!" ''' Send a whole sequence of points to a robot... that is listening to individual joint states. ''' def send_sequence(self,traj,acceleration=0.5,velocity=0.5,cartesian=False): rospy.logerr("Function 'send_sequence' not implemented for base class!") return "FAILURE - running base class!" ''' handle changes in driver mode ''' def handle_mode_tick(self): rospy.logerr("Function 'handle_mode_tick' not implemented for base class!") return "FAILURE - running base class!" ''' Standard movement call. Tries a cartesian move, then if that fails goes into a joint-space move. ''' def servo_to_pose_call(self,req): #rospy.loginfo('Recieved servo to pose request') #print req if self.driver_status == 'SERVO': T = pm.fromMsg(req.target) # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) # inverse kinematics traj = self.planner.getCartesianMove(T,self.q0,self.base_steps,self.steps_per_meter) if len(traj.points) == 0: (code,res) = self.planner.getPlan(req.target,self.q0) # find a non-local movement if not res is None: traj = res.planned_trajectory.joint_trajectory #if len(traj) == 0: # traj.append(self.planner.ik(T,self.q0)) # Send command if len(traj.points) > 0: rospy.logwarn("Robot moving to " + str(traj.points[-1].positions)) return self.send_trajectory(traj,acceleration,velocity) else: rospy.logerr('SIMPLE DRIVER -- IK failed') return 'FAILURE - not in servo mode' else: rospy.logerr('SIMPLE DRIVER -- Not in servo mode') return 'FAILURE - not in servo mode' ''' Standard move call. Make a joint space move to a destination. ''' def servo_to_joints_call(self,req): if self.driver_status == 'SERVO': # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) return 'FAILURE - not yet implemented!' else: rospy.logerr('SIMPLE DRIVER -- Not in servo mode') return 'FAILURE - not in servo mode' ''' set teach mode ''' def set_teach_mode_call(self,req): if req.enable == True: # self.rob.set_freedrive(True) self.driver_status = 'TEACH' return 'SUCCESS - teach mode enabled' else: # self.rob.set_freedrive(False) self.driver_status = 'IDLE' return 'SUCCESS - teach mode disabled' ''' send a single point ''' def send_q(self,pt,acceleration,velocity): pt = JointTrajectoryPoint() pt.positions = self.q0 self.pt_publisher.publish(pt) ''' activate servo mode ''' def set_servo_mode_call(self,req): if req.mode == 'SERVO': self.send_q(self.q0,0.1,0.1) self.driver_status = 'SERVO' return 'SUCCESS - servo mode enabled' elif req.mode == 'DISABLE': self.driver_status = 'IDLE' return 'SUCCESS - servo mode disabled' def shutdown_arm_call(self,req): self.driver_status = 'SHUTDOWN' pass ''' robot-specific logic to update state every "tick" ''' def handle_tick(self): rospy.logerr("Function 'handle_tick' not implemented for base class!") ''' call this when "spinning" to keep updating things ''' def tick(self): self.status_publisher.publish(self.driver_status) self.handle_tick()
def __init__(self, yumi_pb, cfg, exec_thread=True, sim_step_repeat=10): """ Class constructor. Sets up internal motion planning interface for each arm, forward and inverse kinematics solvers, and background threads for updating the position of the robot. Args: yumi_pb (airobot Robot): Instance of PyBullet simulated robot, from airobot library cfg (YACS CfgNode): Configuration parameters exec_thread (bool, optional): Whether or not to start the background joint position control thread. Defaults to True. sim_step_repeat (int, optional): Number of simulation steps to take each time the desired joint position value is updated. Defaults to 10 """ self.cfg = cfg self.yumi_pb = yumi_pb self.sim_step_repeat = sim_step_repeat self.joint_lock = threading.RLock() self._both_pos = self.yumi_pb.arm.get_jpos() self._single_pos = {} self._single_pos['right'] = \ self.yumi_pb.arm.arms['right'].get_jpos() self._single_pos['left'] = \ self.yumi_pb.arm.arms['left'].get_jpos() self.moveit_robot = moveit_commander.RobotCommander() self.moveit_scene = moveit_commander.PlanningSceneInterface() self.moveit_planner = 'RRTConnectkConfigDefault' self.robot_description = '/robot_description' self.urdf_string = rospy.get_param(self.robot_description) self.mp_left = GroupPlanner('left_arm', self.moveit_robot, self.moveit_planner, self.moveit_scene, max_attempts=3, planning_time=5.0, goal_tol=0.5, eef_delta=0.01, jump_thresh=10) self.mp_right = GroupPlanner('right_arm', self.moveit_robot, self.moveit_planner, self.moveit_scene, max_attempts=3, planning_time=5.0, goal_tol=0.5, eef_delta=0.01, jump_thresh=10) self.fk_solver_r = KDLKinematics(URDF.from_parameter_server(), "yumi_body", "yumi_tip_r") self.fk_solver_l = KDLKinematics(URDF.from_parameter_server(), "yumi_body", "yumi_tip_l") self.num_ik_solver_r = trac_ik.IK('yumi_body', 'yumi_tip_r', urdf_string=self.urdf_string) self.num_ik_solver_l = trac_ik.IK('yumi_body', 'yumi_tip_l', urdf_string=self.urdf_string) self.ik_pos_tol = 0.001 # 0.001 working well with pulling self.ik_ori_tol = 0.01 # 0.01 working well with pulling self.execute_thread = threading.Thread(target=self._execute_both) self.execute_thread.daemon = True if exec_thread: self.execute_thread.start() self.step_sim_mode = False else: self.step_sim_mode = True
def execute_move(self, pos): # Close the gripper self._right_gripper.close() self._right_gripper.close() self._right_gripper.close() self._right_gripper.close() self._right_gripper.close() rospy.loginfo('moving') # Get robot parameters and create a limb interface instance robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') limb_interface = baxter_interface.limb.Limb('right') angles = limb_interface.joint_angles() limb_interface.set_joint_position_speed(.7) q_goal = [-.01859, -.5119, 1.7909, 1.232, -1.030, 1.945, -1.31] q0 = kdl_kin.random_joint_angles() current_angles = [ limb_interface.joint_angle(joint) for joint in limb_interface.joint_names() ] for ind in range(len(q0)): q0[ind] = current_angles[ind] q_list = JointTrajectory(q0, np.asarray(q_goal), 1, 50, 5) for q in q_list: for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] limb_interface.set_joint_positions(angles) rospy.sleep(.03) # Create the desired joint trajectory with a quintic time scaling q0 = q_goal # Set the dropoff position to be dropoff #1 if self._goal == 1: q_goal = [-.675, -.445, 1.626, 1.1336, -1.457, 1.6145, -2.190] # Dropoff #2 else: q_goal = [-.066, -.068, 1.738, .8022, -2.23, .917, -2.9057] q_list = JointTrajectory(np.asarray(q0), np.asarray(q_goal), 1, 50, 5) for q in q_list: for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] limb_interface.set_joint_positions(angles) rospy.sleep(.03) # Open the gripper rospy.sleep(.2) self._right_gripper.open() self._right_gripper.open() self._right_gripper.open() self._right_gripper.open() self._right_gripper.open() # Set the position to the old goal and the new goal to the desired camera position for seeing the blocks q0 = q_goal q_goal = [-.01859, -.5119, 1.7909, 1.232, -1.030, 1.945, -1.31] q_list = JointTrajectory(np.asarray(q0), np.asarray(q_goal), 1, 50, 5) for q in q_list: for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] limb_interface.set_joint_positions(angles) rospy.sleep(.05) rospy.sleep(.2) # Publish next state self._pub_state.publish(1) self._done = True print('Done')
class ClutchNGo_node_handler : """ Here is the idea: While camera clutch is being pressed: 1. Find the vector between two of MTML consecutive positions 2. Use forward kinematics to find ECM current position 3. Add the vector to ECM's current position 4. Use inverse kinematics to find joint angles for new position """ class MODE: """ simulation mode: Use the hardware for the master side, and simulation for the ECM hardware mode: Use hardware for both the master side, and the ECM """ simulation = "SIMULATION" hardware = "HARDWARE" def __init__(self): # For forward and inverse kinematics self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name) self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name) self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description') self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name) self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description') self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name) # For camera clutch control self.camera_clutch_pressed = False self.ecm_manual_control_lock_mtml_msg = None self.ecm_manual_control_lock_ecm_msg = None self.mtml_start_position = None self.mtml_end_position = None self.ecm_msg = None self.__clutchNGo_mode__ = self.MODE.simulation self.autocamera = Autocamera() def __init_nodes__(self): # Publishers to the simulation self.ecm_pub = rospy.Publisher('autocamera_node', JointState, queue_size=10) self.psm1_pub = rospy.Publisher('/dvrk_psm1/joint_states_robot', JointState, queue_size=10) self.psm2_pub = rospy.Publisher('/dvrk_psm2/joint_states_robot', JointState, queue_size=10) if self.__clutchNGo_mode__ == self.MODE.simulation: # Get the ECM joint angles from the simulation rospy.Subscriber('/dvrk_ecm/joint_states', JointState, self.ecm_cb) elif self.__clutchNGo_mode__ == self.MODE.hardware: # Get the ECM joint angles from the hardware rospy.Subscriber('/dvrk/ECM/state_joint_current', JointState, self.ecm_cb) # Get the joint angles from MTM hardware rospy.Subscriber('/dvrk/MTML/state_joint_current', JointState, self.mtml_cb) #rospy.Subscriber('/dvrk/MTMR/position_joint_current', JointState, add_psm1_jnt) # Detect whether or not the camera clutch is being pressed rospy.Subscriber('/dvrk/footpedals/camera', Bool, self.camera_clutch_cb) def camera_clutch_cb(self, msg): rospy.logerr('Camera Clutch Pressed : ' + msg.data.__str__()) self.camera_clutch_pressed = msg.data def mtml_cb(self, msg): if self.camera_clutch_pressed == True: if self.current_mtml_pos == None: self.current_mtml_joint_angles = msg.pos else: self.move_ecm( self.current_mtml_joint_angles, msg.pos) self.current_mtml_joint_angles = msg.pos else: self.current_mtml_joint_angles = None def mtmr_cb(self, msg): pass def ecm_cb(self, msg): self.ecm_msg = msg pass def move_ecm(self, first_joint_angles, second_joint_angles): """ 1. Use forward kinematics to determine the 3d coordinates of the two sets of joint angles 2. Find the vector between those coordinates 3. Use that vector to find a new 3d position for the ECM 4. Use inverse kinematics to find joint angles for the ECM 5. Move the ECM to those joint angles """ # (1) start_coordinates,_ = self.mtml_kin.FK( first_joint_angles[:-1]) # Returns (position, rotation) end_coordinates,_ = self.mtml_kin.FK(second_joint_angles[:-1]) # (2) diff = np.subtract(end_coordinates, start_coordinates) # (3) ecm_coordinates,_ = self.ecm_kin.FK(self.ecm_msg.position[0:2] + ecm_msg.position[-2:]) # There are a lot of excessive things here that we don't need ecm_pose = self.ecm_kin.forward(ecm_msg.position[0:2] + ecm_msg.position[-2:]) # Figure out the new orientation and position to be used in the inverse kinematics b,_ = self.ecm_kin.FK([ecm_msg.position[0],ecm_msg.position[1],.14,ecm_msg.position[3]]) keyhole, _ = self.ecm_kin.FK([0,0,0,0]) ecm_current_direction = b-keyhole new_ecm_coordinates = np.add(ecm_coordinates, diff) ecm_new_direction = new_ecm_coordinates - keyhole r = self.autocamera.find_rotation_matrix_between_two_vectors(ecm_current_direction, ecm_new_direction) # (4) ecm_pose[0:3,0:3] = r* ecm_pose[0:3,0:3] ecm_pose[0:3,3] = new_ecm_coordinates new_ecm_joint_angles = self.ecm_kin.inverse(ecm_pose) new_ecm_msg = ecm_msg; new_ecm_msg.position = new_ecm_joint_angles; new_ecm_msg.name = self.ecm_kin.get_joint_names() self.logerror("ecm_new_direction " + ecm_new_direction.__str__()) self.logerror('ecm_coordinates' + ecm_coordinates.__str__()) self.logerror("ecm_pose " + ecm_pose.__str__()) self.logerror("new_ecm_joint_angles " + new_ecm_joint_angles.__str__()) self.logerror("new_ecm_msg" + new_ecm_msg.__str__()) self.ecm_pub.publish(new_ecm_msg) pass def shutdown(self): rospy.signal_shutdown('shutting down clutchNGo') def set_mode(self, mode): """ Values: MODE.simulation MODE.hardware """ self.__clutchNGo_mode__ = mode def spin(self): rospy.spin()
def main(): # Initialize node rospy.init_node('minimization') # Start Moveit Commander InitializeMoveitCommander() # Initialization of KDL Kinematics for right and left grippers robot = URDF.from_xml_file( "/home/josh/catkin_ws/src/baxter_common/baxter_description/urdf/baxter.urdf" ) global kdl_kin_left, kdl_kin_right kdl_kin_left = KDLKinematics(robot, "base", "left_gripper") kdl_kin_right = KDLKinematics(robot, "base", "right_gripper") # Bounds for SLSQP: s0, s1, e0, e1, w0, w1, w2 (left,right) bnds = ((-1.70167993878, 1.70167993878), (-2.147, 1.047), (-3.05417993878, 3.05417993878), (-0.05, 2.618), (-3.059, 3.059), (-1.57079632679, 2.094), (-3.059, 3.059), (-1.70167993878, 1.70167993878), (-2.147, 1.047), (-3.05417993878, 3.05417993878), (-0.05, 2.618), (-3.059, 3.059), (-1.57079632679, 2.094), (-3.059, 3.059) ) # lower and upper bounds for each q (length 14) # Initial guess #x0 = np.full((14,1), 0.75) #x0 = np.array([[random.uniform(bnds[i][0]/2.,bnds[i][1]/2.)] for i in range(14)]) initial_left = Pose() initial_left.position = Point( x=0.3, #(P_left_current[0,0] + P_right_current[0,0])/2., y=(P_left_current[1, 0] + P_right_current[1, 0]) / 2., z=(P_left_current[2, 0] + P_right_current[2, 0]) / 2., ) initial_left.orientation = Quaternion( x=0.0, y=0.0, z=0.0, w=1.0, ) initial_right = Pose() initial_right.position = Point( x=0.3, #(P_left_current[0,0] + P_right_current[0,0])/2., y=(P_left_current[1, 0] + P_right_current[1, 0]) / 2., z=(P_left_current[2, 0] + P_right_current[2, 0]) / 2., ) initial_right.orientation = Quaternion( x=0.0, y=0.0, z=0.0, w=1.0, ) x0_left = kdl_kin_left.inverse(initial_left) x0_right = kdl_kin_left.inverse(initial_right) x0 = np.array([[x0_left[0]], [x0_left[1]], [x0_left[2]], [x0_left[3]], [x0_left[4]], [x0_left[5]], [x0_left[6]], [x0_right[0]], [x0_right[1]], [x0_right[2]], [x0_right[3]], [x0_right[4]], [x0_right[5]], [x0_right[6]]]) x0 = x0 + random.uniform(-0.2, 0.2) # Constraint equality Handoff_separation = np.array([[0.0], [0.1], [0.0], [math.pi / 1.], [0], [-math.pi / 2.]]) cons = ( { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[9, 0] - Handoff_separation[0, 0] }, #x-sep-distance = 0 { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[10, 0] - Handoff_separation[2, 0] }, #y-sep-distance = 0 { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[11, 0] - Handoff_separation[1, 0] }, #z-sep-distance = 0.1 { 'type': 'eq', 'fun': lambda q: math.fabs(JS_to_PrPlRrl(q)[6, 0]) - Handoff_separation[3, 0] }, #roll = pi { 'type': 'eq', 'fun': lambda q: math.fabs(JS_to_PrPlRrl(q)[7, 0]) - Handoff_separation[4, 0] }, #pitch = 0 { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[8, 0] - Handoff_separation[5, 0] }, #yaw = -pi/2 #{'type': 'ineq', 'fun': lambda q: JS_to_PrPlRrl(q)[0,0]-0.3}, #x-pos > 0.3 m to help avoid collisions { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[0, 0] - (P_left_current[0, 0] + P_right_current[0, 0]) / 2. }, { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[1, 0] - (P_left_current[1, 0] + P_right_current[1, 0]) / 2. }) #{'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[0,0] - (P_left_current[0,0] + P_right_current[0,0])/2.}) # Minimization before = rospy.get_rostime() result = minimize(minimization, x0, method='SLSQP', jac=jacobian, bounds=bnds, constraints=cons, tol=0.1, options={'maxiter': 100}) after = rospy.get_rostime() print "\nNumber of iterations: \n", result.success, result.nit print result q_left = result.x[0:7] q_right = result.x[7:14] pose_left = kdl_kin_left.forward(q_left) pose_right = kdl_kin_right.forward(q_right) print "\nPose left: \n", pose_left print "Pose right: \n", pose_right #R_left = pose_left[:3,:3] #R_right = pose_right[:3,:3] #X_left,Y_left,Z_left = euler_from_matrix(R_left, 'sxyz') #X_right,Y_right,Z_right = euler_from_matrix(R_right, 'sxyz') q = np.array([ q_left[0], q_left[1], q_left[2], q_left[3], q_left[4], q_left[5], q_left[6], q_right[0], q_right[1], q_right[2], q_right[3], q_right[4], q_right[5], q_right[6] ]) #P = JS_to_PrPlRrl(q) handoff_distance = math.sqrt((pose_left[0, 3] - pose_right[0, 3])**2 + (pose_left[1, 3] - pose_right[1, 3])**2 + (pose_left[2, 3] - pose_right[2, 3])**2) print "\nDistance between handoff: ", handoff_distance print "Minimization time: ", ( after.secs - before.secs) + (after.nsecs - before.nsecs) / 1e+9 if result.success == False or math.fabs(handoff_distance - 0.1) > 0.05: print "Minimization was a failure." elif result.success == True: print "Minimization was a success." print "Planning a trajectory in MoveIt!" #s0, s1, e0, e1, w0, w1, w2 joints = { 'left_s0': q_left[0], 'left_s1': q_left[1], 'left_e0': q_left[2], 'left_e1': q_left[3], 'left_w0': q_left[4], 'left_w1': q_left[5], 'left_w2': q_left[6], 'right_s0': q_right[0], 'right_s1': q_right[1], 'right_e0': q_right[2], 'right_e1': q_right[3], 'right_w0': q_right[4], 'right_w1': q_right[5], 'right_w2': q_right[6] } group_both_arms.set_joint_value_target(joints) plan_both = group_both_arms.plan() print "Trajectory time (nsec): ", plan_both.joint_trajectory.points[ len(plan_both.joint_trajectory.points) - 1].time_from_start
class Autocamera_node_handler: # move the actual ecm with sliders? __MOVE_ECM_WITH_SLIDERS__ = False class MODE: simulation = "SIMULATION" hardware = "HARDWARE" sliders = "SLIDERS" DEBUG = True def __init__(self): self.t = time.time() self.__AUTOCAMERA_MODE__ = self.MODE.simulation self.autocamera = Autocamera() # Instantiate the Autocamera Class self.jnt_msg = JointState() self.joint_angles = {'ecm':None, 'psm1':None, 'psm2':None} self.cam_info = {'left':CameraInfo(), 'right':CameraInfo()} self.last_ecm_jnt_pos = None self.first_run = True # For forward and inverse kinematics self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name) self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name) self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description') self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name) self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description') self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name) # For camera clutch control self.camera_clutch_pressed = False self.ecm_manual_control_lock_mtml_msg = None self.ecm_manual_control_lock_ecm_msg = None self.mtml_start_position = None self.mtml_end_position = None self.initialize_psms_initialized = 30 self.__DEBUG_GRAPHICS__ = False def __init_nodes__(self): self.ecm_hw = robot('ECM') self.psm1_hw = robot('PSM1') self.psm2_hw = robot('PSM2') #rospy.init_node('autocamera_node') self.logerror("start", debug=True) # Publishers to the simulation self.ecm_pub = rospy.Publisher('/dvrk_ecm/joint_states_robot', JointState, queue_size=10) self.psm1_pub = rospy.Publisher('/dvrk_psm1/joint_states_robot', JointState, queue_size=10) self.psm2_pub = rospy.Publisher('/dvrk_psm2/joint_states_robot', JointState, queue_size=10) # Get the joint angles from the simulation rospy.Subscriber('/dvrk_ecm/joint_states', JointState, self.add_ecm_jnt) rospy.Subscriber('/fakecam_node/camera_info', CameraInfo, self.get_cam_info) try: self.sub_psm1_sim.unregister() self.sub_psm2_sim.unregister() self.sub_psm1_hw.unregister() self.sub_psm2_hw.unregister() except Exception: pass if self.__AUTOCAMERA_MODE__ == self.MODE.hardware : # Get the joint angles from the hardware and move the simulation from hardware self.sub_psm1_hw = rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, self.add_psm1_jnt) self.sub_psm2_hw = rospy.Subscriber('/dvrk/PSM2/state_joint_current', JointState, self.add_psm2_jnt) elif self.__AUTOCAMERA_MODE__ == self.MODE.simulation: # Get the joint angles from the simulation self.sub_psm1_sim = rospy.Subscriber('/dvrk_psm1/joint_states', JointState, self.add_psm1_jnt) self.sub_psm2_sim = rospy.Subscriber('/dvrk_psm2/joint_states', JointState, self.add_psm2_jnt) # If hardware is connected, subscribe to it and set the psm joint angles in the simulation from the hardware self.sub_psm1_hw = rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, self.add_psm1_jnt_from_hw) self.sub_psm2_hw = rospy.Subscriber('/dvrk/PSM2/state_joint_current', JointState, self.add_psm2_jnt_from_hw) # Get the joint angles from MTM hardware ##rospy.Subscriber('/dvrk/MTML/position_joint_current', JointState, self.mtml_cb) # rospy.Subscriber('/dvrk/MTMR/position_joint_current', JointState, add_psm1_jnt) # Detect whether or not the camera clutch is being pressed ## rospy.Subscriber('/dvrk/footpedals/camera', Bool, self.camera_clutch_cb) # Move the hardware from the simulation # rospy.Subscriber('/dvrk_psm1/joint_states', JointState, self.move_psm1) # rospy.Subscriber('/dvrk_psm2/joint_states', JointState, self.move_psm2) if self.__DEBUG_GRAPHICS__ == True: # Subscribe to fakecam images rospy.Subscriber('/fakecam_node/fake_image_left', Image, self.left_image_cb) rospy.Subscriber('/fakecam_node/fake_image_right', Image, self.right_image_cb) # Publish images self.image_left_pub = rospy.Publisher('autocamera_image_left', Image, queue_size=10) self.image_right_pub = rospy.Publisher('autocamera_image_right', Image, queue_size=10) def shutdown(self): rospy.signal_shutdown('shutting down Autocamera') # This needs to be run before anything can be expected def spin(self): self.__init_nodes__() # initialize all the nodes, subscribers and publishers rospy.spin() def debug_graphics(self, has_graphics): self.__DEBUG_GRAPHICS__ = has_graphics def logerror(self, msg, debug = False): if self.DEBUG or debug: rospy.logerr(msg) def ecm_manual_control_lock(self, msg, fun): if fun == 'ecm': self.ecm_manual_control_lock_ecm_msg = msg elif fun == 'mtml': self.ecm_manual_control_lock_mtml_msg = msg if self.ecm_manual_control_lock_mtml_msg != None and self.ecm_manual_control_lock_ecm_msg != None: self.ecm_manual_control(self.ecm_manual_control_lock_mtml_msg, self.ecm_manual_control_lock_ecm_msg) self.ecm_manual_control_lock_mtml_msg = None self.ecm_manual_control_lock_ecm_msg = None def ecm_manual_control(self, mtml_msg, ecm_msg): # TODO: Find forward kinematics from mtmr and ecm. Move mtmr. Find the movement vector. Add it to the # ecm position, use inverse kinematics and move the ecm. self.logerror("mtml" + self.mtml_kin.get_joint_names().__str__()) start_coordinates,_ = self.mtml_kin.FK( self.mtml_start_position.position[:-1]) # Returns (position, rotation) end_coordinates,_ = self.mtml_kin.FK(mtml_msg.position[:-1]) diff = np.subtract(end_coordinates, start_coordinates) self.logerror("diff = " + diff.__str__()) # Find the ecm 3d coordinates, add the 'diff' to it, then do an inverse kinematics ecm_coordinates,_ = self.ecm_kin.FK(ecm_msg.position[0:2] + ecm_msg.position[-2:]) # There are a lot of excessive things here that we don't need ecm_pose = self.ecm_kin.forward(ecm_msg.position[0:2] + ecm_msg.position[-2:]) # Figure out the new orientation and position to be used in the inverse kinematics b,_ = self.ecm_kin.FK([ecm_msg.position[0],ecm_msg.position[1],.14,ecm_msg.position[3]]) keyhole, _ = self.ecm_kin.FK([0,0,0,0]) ecm_current_direction = b-keyhole new_ecm_coordinates = np.add(ecm_coordinates, diff) ecm_new_direction = new_ecm_coordinates - keyhole r = self.autocamera.find_rotation_matrix_between_two_vectors(ecm_current_direction, ecm_new_direction) ecm_pose[0:3,0:3] = r* ecm_pose[0:3,0:3] ecm_pose[0:3,3] = new_ecm_coordinates new_ecm_joint_angles = self.ecm_kin.inverse(ecm_pose) new_ecm_msg = ecm_msg; new_ecm_msg.position = new_ecm_joint_angles; new_ecm_msg.name = self.ecm_kin.get_joint_names() self.logerror("ecm_new_direction " + ecm_new_direction.__str__()) self.logerror('ecm_coordinates' + ecm_coordinates.__str__()) self.logerror("ecm_pose " + ecm_pose.__str__()) self.logerror("new_ecm_joint_angles " + new_ecm_joint_angles.__str__()) self.logerror("new_ecm_msg" + new_ecm_msg.__str__()) self.ecm_pub.publish(new_ecm_msg) def camera_clutch_cb(self, msg): self.camera_clutch_pressed = msg.data self.logerror('Camera Clutch : ' + self.camera_clutch_pressed.__str__()) def mtml_cb(self, msg): if self.camera_clutch_pressed : if self.mtml_start_position == None: self.mtml_start_position = msg self.mtml_end_position = msg #Freeze mtms ### CODE HERE ### # move the ecm based on the movement of MTML self.ecm_manual_control_lock(msg, 'mtml') else: self.mtml_start_position = msg self.mtml_end_position = None def move_psm1(self, msg): jaw = msg.position[-3] msg = self.autocamera.extract_positions(msg, 'psm1') msg.name = msg.name + ['jaw'] msg.position = msg.position + [jaw] self.psm1_hw.move_joint_list(msg.position, interpolate=True) def move_psm2(self, msg): time.sleep(.5) jaw = msg.position[-3] msg = self.autocamera.extract_positions(msg, 'psm2') msg.name = msg.name + ['jaw'] msg.position = msg.position + [jaw] self.psm2_hw.move_joint_list(msg.position, interpolate=first_run) # ecm callback def add_ecm_jnt(self, msg): if self.camera_clutch_pressed == False and msg != None: if self.__MOVE_ECM_WITH_SLIDERS__ == False: self.add_jnt('ecm', msg) else: temp = list(msg.position[:2]+msg.position[-2:]) r = self.ecm_hw.move_joint_list(temp, interpolate=first_run) if r == True: self.first_run = False else: self.ecm_manual_control_lock(msg, 'ecm') def add_psm1_jnt_from_hw(self, msg): if self.camera_clutch_pressed == False and msg != None: if self.__AUTOCAMERA_MODE__ == self.MODE.simulation: msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw'] self.psm1_pub.publish(msg) def add_psm2_jnt_from_hw(self, msg): if self.camera_clutch_pressed == False and msg != None: if self.__AUTOCAMERA_MODE__ == self.MODE.simulation: msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw'] self.psm2_pub.publish(msg) # psm1 callback def add_psm1_jnt(self, msg): if self.camera_clutch_pressed == False and msg != None: # We need to set the names, otherwise the simulation won't move if self.__AUTOCAMERA_MODE__ == self.MODE.hardware: msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw'] self.psm1_pub.publish(msg) self.add_jnt('psm1', msg) # psm2 callback def add_psm2_jnt(self, msg): if self.camera_clutch_pressed == False and msg != None: if self.__AUTOCAMERA_MODE__ == self.MODE.hardware : msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw'] self.psm2_pub.publish(msg) self.add_jnt('psm2', msg) def add_jnt(self, name, msg): self.joint_angles[name] = msg if not None in self.joint_angles.values(): if self.initialize_psms_initialized>0 and self.__AUTOCAMERA_MODE__ == self.MODE.simulation: self.initialize_psms() time.sleep(.01) self.initialize_psms_initialized -= 1 try: jnt_msg = 'error' jnt_msg = self.autocamera.compute_viewangle(self.joint_angles, self.cam_info) self.ecm_pub.publish(jnt_msg) jnt_msg.position = [ round(i,4) for i in jnt_msg.position] if len(jnt_msg.position) != 4 or len(jnt_msg.name) != 4 : return #return # stop here until we co-register the arms if self.__AUTOCAMERA_MODE__ == self.MODE.hardware: pos = jnt_msg.position result = self.ecm_hw.move_joint_list(pos, index=[0,1,2,3], interpolate=self.first_run) # Interpolate the insertion joint individually and the rest without interpolation # pos = [jnt_msg.position[2]] # result = self.ecm_hw.move_joint_list(pos, index=[2], interpolate=True) if result: self.first_run = False # # except TypeError: # rospy.logerr('Exception : ' + TypeError.message.__str__()) pass self.joint_angles = dict.fromkeys(self.joint_angles, None) # camera info callback def get_cam_info(self, msg): if msg.header.frame_id == '/fake_cam_left_optical_link': self.cam_info['left'] = msg elif msg.header.frame_id == '/fake_cam_right_optical_link': self.cam_info['right'] = msg def image_cb(self, image_msg, camera_name): image_pub = {'left':self.image_left_pub, 'right':self.image_right_pub}[camera_name] bridge = cv_bridge.CvBridge() im = bridge.imgmsg_to_cv2(image_msg, 'rgb8') tool1_name = {'left':'l1', 'right':'r1'} tool2_name = {'left':'l2', 'right':'r2'} toolm_name = {'left':'lm', 'right':'rm'} tool1 = self.autocamera.zoom_level_positions[tool1_name[camera_name]]; tool1 = tuple(int(i) for i in tool1) tool2 = self.autocamera.zoom_level_positions[tool2_name[camera_name]]; tool2 = tuple(int(i) for i in tool2) toolm = self.autocamera.zoom_level_positions[toolm_name[camera_name]]; toolm = tuple(int(i) for i in toolm) rotate_180 = lambda p : (640-p[0], 480-p[2]) cv2.circle(im, tool1, 10, (0,255,0), -1) cv2.circle(im, tool2, 10, (0,255,255), -1) cv2.circle(im, toolm, 10, (0,0,255), -1) cv2.circle(im, (0,0), 20, (255,0,0), -1) cv2.circle(im, (640,480), 20, (255,0,255), -1) new_image = bridge.cv2_to_imgmsg(im, 'rgb8') new_image.header.seq = image_msg.header.seq new_image.header.stamp = image_msg.header.stamp new_image.header.frame_id = image_msg.header.frame_id image_pub.publish(new_image) def left_image_cb(self, image_msg): self.image_cb(image_msg, 'left') def right_image_cb(self, image_msg): self.image_cb(image_msg, 'right') def initialize_psms(self): if self.__AUTOCAMERA_MODE__ == self.MODE.simulation : msg = JointState() msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw'] msg.position = [0.84 , -0.65, 0.10, 0.00, 0.00, 0.00, 0.00] self.psm1_pub.publish(msg) msg.position = [-0.84 , -0.53, 0.10, 0.00, 0.00, 0.00, 0.00] self.psm2_pub.publish(msg) self.logerror('psms initialized!') def set_mode(self, mode): """ Values: MODE.simulation MODE.hardware """ self.__AUTOCAMERA_MODE__ = mode
class SimplePlanning: def __init__(self, robot, base_link, end_link, group, move_group_ns="move_group", planning_scene_topic="planning_scene", robot_ns="", verbose=False, kdl_kin=None, closed_form_IK_solver=None, joint_names=[]): self.robot = robot self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if kdl_kin is None: self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) else: self.kdl_kin = kdl_kin self.base_link = base_link self.joint_names = joint_names self.end_link = end_link self.group = group self.robot_ns = robot_ns self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction) self.verbose = verbose self.closed_form_IK_solver = closed_form_IK_solver ''' ik: handles calls to KDL inverse kinematics ''' def ik(self, T, q0, dist=0.5): q = None if self.closed_form_IK_solver is not None: #T = pm.toMatrix(F) q = self.closed_form_IK_solver.findClosestIK(T, q0) else: q = self.kdl_kin.inverse(T, q0) # NOTE: this results in unsafe behavior; do not use without checks #if q is None: # q = self.kdl_kin.inverse(T) return q ''' TODO: finish this ''' def getCartesianMove(self, frame, q0, base_steps=1000, steps_per_meter=1000, time_multiplier=10): # interpolate between start and goal pose = pm.fromMatrix(self.kdl_kin.forward(q0)) cur_rpy = np.array(pose.M.GetRPY()) cur_xyz = np.array(pose.p) goal_rpy = np.array(frame.M.GetRPY()) goal_xyz = np.array(frame.p) steps = base_steps + int((pose.p - frame.p).Norm() * steps_per_meter) print " -- Computing %f steps" % steps ts = (pose.p - frame.p).Norm() / steps * time_multiplier traj = JointTrajectory() traj.points.append( JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0))) # compute IK for i in range(1, steps + 1): xyz = cur_xyz + ((float(i) / steps) * (goal_xyz - cur_xyz)) rpy = cur_rpy + ((float(i) / steps) * (goal_rpy - cur_rpy)) frame = pm.toMatrix( kdl.Frame(kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]), kdl.Vector(xyz[0], xyz[1], xyz[2]))) #q = self.kdl_kin.inverse(frame,q0) q = self.ik(frame, q0) if self.verbose: print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q)) if not q is None: pt = JointTrajectoryPoint(positions=q, velocities=[0] * len(q), accelerations=[0] * len(q)) pt.time_from_start = rospy.Duration(i * ts) traj.points.append(pt) q0 = q elif i == steps: return JointTrajectory() if len(traj.points) < base_steps: print rospy.logerr("Planning failure with " \ + str(len(traj.points)) \ + " / " + str(base_steps) \ + " points.") return JointTrajectory() traj.joint_names = self.joint_names return traj def getGoalConstraints(self, frames, q, timeout=2.0, mode=ModeJoints): is_list_of_frame = isinstance(frames, list) number_of_frames = None if len(self.robot_ns) > 0: srv = rospy.ServiceProxy(self.robot_ns + "/compute_ik", moveit_msgs.srv.GetPositionIK) else: srv = rospy.ServiceProxy("compute_ik", moveit_msgs.srv.GetPositionIK) goal = Constraints() if is_list_of_frame: number_of_frames = len(frames) else: number_of_frames = 1 # print "Start looping thru cartesian points" for i in xrange(number_of_frames): frame = None joints = None previous_q = None if is_list_of_frame: print "is a list of frames" frame = frames[i] else: "just one frame" frame = frames if i == 0: previous_q = q joints = self.ik(pm.toMatrix(frame), previous_q) if joints is not None: previous_q = joints else: return (None, None) # print joints is None # print joints # if joints is None: # p = geometry_msgs.msg.PoseStamped() # p.pose.position.x = frame.position.x # p.pose.position.y = frame.position.y # p.pose.position.z = frame.position.z # p.pose.orientation.x = frame.orientation.x # p.pose.orientation.y = frame.orientation.y # p.pose.orientation.z = frame.orientation.z # p.pose.orientation.w = frame.orientation.w # p.header.frame_id = "/world" # ik_req = moveit_msgs.msg.PositionIKRequest() # ik_req.robot_state.joint_state.name = self.joint_names # ik_req.robot_state.joint_state.position = q # ik_req.avoid_collisions = True # ik_req.timeout = rospy.Duration(timeout) # ik_req.attempts = 5 # ik_req.group_name = self.group # ik_req.pose_stamped = p # rospy.logwarn("Getting IK position...") # ik_resp = srv(ik_req) # rospy.logwarn("IK RESULT ERROR CODE = %d"%(ik_resp.error_code.val)) # #if ik_resp.error_code.val > 0: # # return (ik_resp, None) # #print ik_resp.solution # ############################### # # now create the goal based on inverse kinematics # if not ik_resp.error_code.val < 0: # for i in range(0,len(ik_resp.solution.joint_state.name)): # print ik_resp.solution.joint_state.name[i] # print ik_resp.solution.joint_state.position[i] # joint = JointConstraint() # joint.joint_name = ik_resp.solution.joint_state.name[i] # joint.position = ik_resp.solution.joint_state.position[i] # joint.tolerance_below = 0.005 # joint.tolerance_above = 0.005 # joint.weight = 1.0 # goal.joint_constraints.append(joint) # return (ik_resp, goal) if mode == ModeJoints: for i in range(0, len(self.joint_names)): joint = JointConstraint() joint.joint_name = self.joint_names[i] joint.position = joints[i] joint.tolerance_below = 0.005 joint.tolerance_above = 0.005 joint.weight = 1.0 goal.joint_constraints.append(joint) else: print 'Cartesian Constraint', self.base_link, self.end_link # TODO: Try to fix this again. Something is wrong position_costraint = PositionConstraint() position_costraint.link_name = self.end_link position_costraint.target_point_offset.x = 0.0 position_costraint.target_point_offset.y = 0.0 position_costraint.target_point_offset.z = 0.0 sphere_bounding = SolidPrimitive() sphere_bounding.type = sphere_bounding.SPHERE sphere_bounding.dimensions.append(0.005) position_costraint.constraint_region.primitives.append( sphere_bounding) sphere_pose = Pose() sphere_pose.position = frame.p sphere_pose.orientation.x = 0.0 sphere_pose.orientation.y = 0.0 sphere_pose.orientation.z = 0.0 sphere_pose.orientation.w = 1.0 position_costraint.constraint_region.primitive_poses.append( sphere_pose) position_costraint.weight = 1.0 position_costraint.header.frame_id = '/world' goal.position_constraints.append(position_costraint) orientation_costraint = OrientationConstraint() orientation_costraint.link_name = self.end_link orientation_costraint.header.frame_id = '/world' orientation_costraint.orientation = frame.M.GetQuaternion() orientation_costraint.absolute_x_axis_tolerance = 0.005 orientation_costraint.absolute_y_axis_tolerance = 0.005 orientation_costraint.absolute_z_axis_tolerance = 0.005 orientation_costraint.weight = 1.0 goal.orientation_constraints.append(orientation_costraint) return (None, goal) def updateAllowedCollisions(self, obj, allowed): self.planning_scene_publisher = rospy.Publisher( 'planning_scene', PlanningScene) rospy.wait_for_service('/get_planning_scene', 10.0) get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) request = PlanningSceneComponents( components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = get_planning_scene(request) acm = response.scene.allowed_collision_matrix if not obj in acm.default_entry_names: # add button to allowed collision matrix acm.default_entry_names += [obj] acm.default_entry_values += [allowed] else: idx = acm.default_entry_names.index(obj) acm.default_entry_values[idx] = allowed planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) self.planning_scene_publisher.publish(planning_scene_diff) def getPlan(self, frame, q, obj=None, compute_ik=True): planning_options = PlanningOptions() planning_options.plan_only = True planning_options.replan = False planning_options.replan_attempts = 0 planning_options.replan_delay = 0.1 planning_options.planning_scene_diff.is_diff = True planning_options.planning_scene_diff.robot_state.is_diff = True if obj is not None: self.updateAllowedCollisions(obj, True) motion_req = MotionPlanRequest() motion_req.start_state.joint_state.position = q motion_req.workspace_parameters.header.frame_id = self.base_link motion_req.workspace_parameters.max_corner.x = 1.0 motion_req.workspace_parameters.max_corner.y = 1.0 motion_req.workspace_parameters.max_corner.z = 1.0 motion_req.workspace_parameters.min_corner.x = -1.0 motion_req.workspace_parameters.min_corner.y = -1.0 motion_req.workspace_parameters.min_corner.z = -1.0 # create the goal constraints # TODO: change this to use cart goal(s) # - frame: take a list of frames # - returns: goal contraints if compute_ik: (ik_resp, goal) = self.getGoalConstraints(frame, q, mode=ModeJoints) #if (ik_resp.error_code.val > 0): # return (1,None) print 'IK error code: ', ik_resp # print goal motion_req.goal_constraints.append(goal) motion_req.group_name = self.group motion_req.num_planning_attempts = 10 motion_req.allowed_planning_time = 4.0 motion_req.planner_id = "RRTConnectkConfigDefault" if goal is None or len( motion_req.goal_constraints[0].joint_constraints) == 0 or ( not ik_resp is None and ik_resp.error_code.val < 0): return (-31, None) goal = MoveGroupGoal() goal.planning_options = planning_options goal.request = motion_req rospy.logwarn("Sending request...") self.client.send_goal(goal) self.client.wait_for_result() res = self.client.get_result() rospy.logwarn("Done: " + str(res.error_code.val)) if obj is not None: self.updateAllowedCollisions(obj, False) return (res.error_code.val, res)
def main(): rospy.init_node('set_base_frames') sleep (1) global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot if psm1_robot is None: psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name) if psm2_robot is None: psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description') psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name) if ecm_robot is None: ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name) ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name) # pdb.set_trace() mtml_pub = rospy.Publisher('/dvrk/MTML/set_base_frame', Pose, queue_size=1) mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, queue_size=1) ecm_pub = rospy.Publisher('/dvrk/ECM/set_base_frame', Pose, queue_size=1) psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, queue_size=1) psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, queue_size=1) mtmr_psm1 = rospy.Publisher('/dvrk/MTMR_PSM1/set_registration_rotation', Quaternion, queue_size=1) mtml_psm2 = rospy.Publisher('/dvrk/MTML_PSM2/set_registration_rotation', Quaternion, queue_size=1) p1_base = psm1_kin.forward([]) # PSM1 Base Frame p2_base = psm2_kin.forward([]) # PSM2 Base Frame e_base = ecm_base.forward([]) # ECM Base Frame e = ecm_kin.forward([0,0,0,0]) # ECM Tool Tip camera_view_transform = pose_converter.PoseConv.to_homo_mat( [(0.0,0.0,0.0), (1.57079632679, 0.0, 0.0)]) r = lambda axis, rad: rotate(axis, rad) mtmr_m = e;# mtmr_m = mtmr_m**-1 mtml_m = e print 'qmat' qmsg = Quaternion() temp = quat_from_homomat(p1_base) print quat_from_homomat(p1_base) message = pose_from_homomat(p1_base); psm1_pub.publish(message) while not rospy.is_shutdown(): #print p1_base #print message psm1_pub.publish(message) print ('sure\n') # psm2_pub.publish(pose_from_homomat(p2_base)) # psm1_pub.publish(pose_from_homomat(e_base)) # mtml_pub.publish( pose_from_homomat(mtml_m)) # mtmr_pub.publish( pose_from_homomat(mtmr_m)) print ('\n\Hello: nmtml rotation: \n') print( mtml_m.__repr__( )) print(pose_from_homomat(mtml_m)) print ('\n\nmtmr rotation: \n') print( mtmr_m.__repr__())
class ReachingGame: def __init__(self, robot): self.reaching_bound_pos = 0.03 self.reaching_bound_rot = 0.3 self.reaching_target = [ [0.5, 0.05, 0.8, -1.0, 0.0, 0.0, 6.123233995736766e-17], [0.5, 0, 0.9, -1.0, 0.0, 0.0, 6.123233995736766e-17], [0.45, 0.15, 0.9, -1.0, 0.0, 0.0, 6.123233995736766e-17], [ 0.4, 0.1, 0.9, -0.9659258262890683, 1.5848e-17, -0.2588, 5.91e-17 ], [ 0.4, 0.1, 0.9, -0.8660254037844387, 3.0616e-17, -0.4999, 5.30e-17 ], [ 0.45, 0.15, 0.9, -0.7500000000000001, -0.24999999999999986, -0.4330127018922193, -0.4330127018922192 ] ] self.num_target = len(self.reaching_target) self.CubeState = LinkState() self.CubeState.link_name = "jaco_on_table::Cube3" self.CubeState.reference_frame = "jaco_on_table::robot_base" cube_topic = "/gazebo/set_link_state" self.pub_cube = rospy.Publisher(cube_topic, gazebo_msgs.msg.LinkState, queue_size=1) self.kdl_kin = KDLKinematics(robot, "robot_base", "jaco_fingers_base_link") self.current_target = 0 self.end_game = False self.time_taken = [0] * self.num_target self.current_time = time.time() def game_start(self): self.set_target(self.current_target) def set_target(self, i): self.CubeState.pose.position.x = self.reaching_target[i][0] self.CubeState.pose.position.y = self.reaching_target[i][1] self.CubeState.pose.position.z = self.reaching_target[i][2] self.CubeState.pose.orientation.x = self.reaching_target[i][3] self.CubeState.pose.orientation.y = self.reaching_target[i][4] self.CubeState.pose.orientation.z = self.reaching_target[i][5] self.CubeState.pose.orientation.w = self.reaching_target[i][6] self.pub_cube.publish(self.CubeState) print "Target set to ", self.current_target def test_reached(self, currentJointState): self.pub_cube.publish(self.CubeState) # test if the current position is reached within a bound cur_pose = self.kdl_kin.forward(currentJointState) cur_q = RotationMatrix2Quaternion(cur_pose[0:3, 0:3]) current = [ cur_pose[0, 3].item(0), cur_pose[1, 3].item(0), cur_pose[2, 3].item(0), cur_q[0], cur_q[1], cur_q[2], cur_q[3] ] target = self.reaching_target[self.current_target] # print target[0] - current[0], target[1] - current[1], target[2] - current[2] for i in range(3): if np.abs(target[i] - current[i]) > self.reaching_bound_pos: return target_rot = Quaternion2EulerXYZ(target[3:]) current_rot = Quaternion2EulerXYZ(current[3:]) # test_rot = rotx33(current_rot[0]) * roty33(current_rot[1]) * rotz33(current_rot[2]) # print test_rot - cur_pose[0:3, 0:3] print target_rot[0] - current_rot[0], target_rot[1] - current_rot[1] # print current_rot for i in range(2): # print np.abs(np.min([target_rot[i] - current_rot[i], target_rot[i] - current_rot[i] - 2*np.pi, # target_rot[i] - current_rot[i] + 2*np.pi])) if np.min([ np.abs(target_rot[i] - current_rot[i]), np.abs(target_rot[i] - current_rot[i] - 2 * np.pi), np.abs(target_rot[i] - current_rot[i] + 2 * np.pi) ]) > self.reaching_bound_rot: return # passed all test # record time taken self.time_taken[int( self.current_target)] = time.time() - self.current_time self.current_time = time.time() self.current_target = self.current_target + 1 if self.current_target == self.num_target: self.end_game = True return self.set_target(self.current_target) def log_data(self, IK_method, subject): with open("../TimeTaken.csv", 'a') as writeFile: writer = csv.writer(writeFile) data = [IK_method] for i in range(self.num_target): data.append(str(self.time_taken[i])) data.append(subject) data.append(str(datetime.datetime.now())) writer.writerow(data)