def get_box_plan(self, num, start_state, grasp): # Set argument start state moveit_start_state = RobotState() moveit_start_state.joint_state = start_state self.arm.set_start_state(moveit_start_state) # Calculate goal pose self.arm.set_joint_value_target(self.box_pose[num]) # plan plan = RobotTrajectory() counter = 0 while len(plan.joint_trajectory.points) == 0: plan = self.arm.plan() counter += 1 self.arm.set_planning_time(self.planning_limitation_time + counter * 5.0) if counter > 1: return (False, start_state) self.arm.set_planning_time(self.planning_limitation_time) rospy.loginfo("!! Got a box plan !!") # publish the plan pub_msg = HandringPlan() pub_msg.grasp = grasp pub_msg.trajectory = plan self.hp_pub.publish(pub_msg) self.arm.clear_pose_targets() # return goal state from generated trajectory goal_state = JointState() goal_state.header = Header() goal_state.header.stamp = rospy.Time.now() goal_state.name = plan.joint_trajectory.joint_names[:] goal_state.position = plan.joint_trajectory.points[-1].positions[:] return (True, goal_state, plan)
def js_cb(self, a): rospy.loginfo('Received array') js = JointState() js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2'] jList = a.data jMatrix = np.reshape(jList,(jList.shape[0]/15,15)) i = 0 for pos in jMatrix: rospy.loginfo(pos) js.position = pos gsvr = GetStateValidityRequest() rs = RobotState() rs.joint_state = js gsvr.robot_state = rs gsvr.group_name = "both_arms" resp = self.coll_client(gsvr) if (resp.valid == False): rospy.loginfo('false') rospy.loginfo(i) self.js_pub.publish(0) return i = i + 1 self.js_pub.publish(1) rospy.loginfo('true') return
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") 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)
def getIkPose(self, pose, group="right_arm", previous_state=None): """Get IK of the pose specified, for the group specified, optionally using the robot_state of previous_state (if not, current robot state will be requested) """ # point point to test if there is ik # returns the answer of the service rqst = GetPositionIKRequest() rqst.ik_request.avoid_collisions = True rqst.ik_request.group_name = group rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now()) rqst.ik_request.pose_stamped.header.frame_id = 'base_link' # Set point to check IK for rqst.ik_request.pose_stamped.pose.position = pose.position rqst.ik_request.pose_stamped.pose.orientation = pose.orientation if previous_state == None: current_joint_state = rospy.wait_for_message( DEFAULT_JOINT_STATES, JointState) cs = RobotState() cs.joint_state = current_joint_state #cs = self.r_commander.get_current_state() # old code rqst.ik_request.robot_state = cs else: rqst.ik_request.robot_state = previous_state ik_answer = GetPositionIKResponse() if DEBUG_MODE: timeStart = rospy.Time.now() ik_answer = self.ik_serv.call(rqst) if DEBUG_MODE: durationCall = rospy.Time.now() - timeStart rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s") return ik_answer
def move_group_python_interface_tutorial(max_planning_time=100): global robot global scene global group moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("arm_group") group.clear_pose_targets() joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] joint_state.position = [ -0.399785047899, 0.547865794198, -0.53408570763, -1.53819544195, -0.400005105505, -4.7477817452 ] moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) group.set_joint_value_target([ 0.41523042671, 0.740434785089, -1.26737863948, -3.86136759133, -0.658526198241, -2.53533941067 ]) group.set_planning_time(max_planning_time) plan2 = group.plan() group.set_pose_targets() moveit_commander.roscpp_shutdown()
def publish_robot_state(robot_state_publisher, state, state_validity_checker=None, duration=1.0, display_status='all'): robot_state = RobotState() robot_state.joint_state = convertStateToJointState( state) # convert this to a way-point first # robot_state.is_diff = False robot_state_msg = DisplayRobotState() robot_state_msg.state = robot_state if display_status != 'all': result = state_validity_checker.getStateValidity( robot_state, group_name='both_arms_torso', constraints=None) # print("The result for validity check is: %s" % result) if display_status == 'valid' and result.valid or ( display_status == 'inValid' and not result.valid): robot_state_publisher.publish(robot_state_msg) time.sleep(duration) if not result.valid: print("The result for validity check is: %s" % result) return True else: robot_state_publisher.publish(robot_state_msg) time.sleep(duration) return True return False
def __init__(self, name, hand_or_arm="both", hand_h=False): self.__save = rospy.ServiceProxy('save_robot_state', SaveState) self.__name = name self.__hand_h = hand_h if hand_or_arm == "arm": self.__commander = SrArmCommander() elif hand_or_arm == 'hand': if self.__hand_h: self.__commander = SrRobotCommander("hand_h", prefix="H1_") else: hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() hand_serial = hand_parameters.mapping.keys()[0] self.__commander = SrHandCommander( hand_parameters=hand_parameters, hand_serial=hand_serial) else: self.__arm_commander = SrArmCommander() if self.__hand_h: self.__hand_commander = SrRobotCommander("hand_h", prefix="H1_") else: hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() hand_serial = hand_parameters.mapping.keys()[0] self.__hand_commander = SrHandCommander( hand_parameters=hand_parameters, hand_serial=hand_serial) self.__hand_or_arm = hand_or_arm rs = RobotState() current_dict = {} if self.__hand_or_arm == "both": current_dict = self.__arm_commander.get_robot_state_bounded() robot_name = self.__arm_commander.get_robot_name() elif self.__hand_or_arm == "arm": current_dict = self.__commander.get_current_state_bounded() robot_name = self.__commander.get_robot_name() elif self.__hand_or_arm == "hand": current_dict = self.__commander.get_current_state_bounded() robot_name = self.__commander.get_robot_name() else: rospy.logfatal("Unknown save type") exit(-1) rospy.loginfo(current_dict) rs.joint_state = JointState() rs.joint_state.name = current_dict.keys() rs.joint_state.position = current_dict.values() self.__save(self.__name, robot_name, rs)
def collision_free_plan(self, joint_start, joint_target, initial_trajectory=None, start_value=0): ''' uses Moveit and OMPL to plan a path and generate the trajectory. The trajectory is sent point by point to the robot. A final message is sent to signify the end of the trajectory and to trigger the motion. ''' joint_state = JointState() joint_state.header.stamp = rospy.Time.now() joint_state.name = self.group.get_joints()[:-1] joint_state.position = joint_start robot_state = RobotState() robot_state.joint_state = joint_state self.group.set_start_state(robot_state) self.group.set_joint_value_target(joint_target) if initial_trajectory is not None: initial_trajectory.joint_trajectory.points = initial_trajectory.joint_trajectory.points[ start_value:] self.initial_trajectory_proxy( initial_trajectory.joint_trajectory, 1, len(initial_trajectory.joint_trajectory.points) - 2) else: self.initial_trajectory_proxy(JointTrajectory(), -1, -1) self.group.set_workspace([-3, -3, -3, 3, 3, 3]) plan = self.group.plan() return plan
def getIkPose(self, pose, group="right_arm", previous_state=None): """Get IK of the pose specified, for the group specified, optionally using the robot_state of previous_state (if not, current robot state will be requested) """ # point point to test if there is ik # returns the answer of the service rqst = GetPositionIKRequest() rqst.ik_request.avoid_collisions = True rqst.ik_request.group_name = group rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now()) rqst.ik_request.pose_stamped.header.frame_id = 'base_link' # Set point to check IK for rqst.ik_request.pose_stamped.pose.position = pose.position rqst.ik_request.pose_stamped.pose.orientation = pose.orientation if previous_state == None: current_joint_state = rospy.wait_for_message(DEFAULT_JOINT_STATES, JointState) cs = RobotState() cs.joint_state = current_joint_state #cs = self.r_commander.get_current_state() # old code rqst.ik_request.robot_state = cs else: rqst.ik_request.robot_state = previous_state ik_answer = GetPositionIKResponse() if DEBUG_MODE: timeStart = rospy.Time.now() ik_answer = self.ik_serv.call(rqst) if DEBUG_MODE: durationCall= rospy.Time.now() - timeStart rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s") return ik_answer
def move_to_joint_position(self, joints): """ Adds the given joint values to the current joint values, moves to position """ joint_state = self.joint_state joint_dict = dict(zip(joint_state.name, joint_state.position)) for i in range(len(JOINT_NAMES)): joint_dict[JOINT_NAMES[i]] += joints[i] joint_state = JointState() joint_state.name = JOINT_NAMES joint_goal = [joint_dict[joint] for joint in JOINT_NAMES] joint_goal = np.clip(np.array(joint_goal), JOINT_MIN, JOINT_MAX) joint_state.position = joint_goal header = Header() robot_state = RobotState() robot_state.joint_state = joint_state link_names = ['ee_link'] position = self.get_fk_client(header, link_names, robot_state) target_p = position[0].pose.position x, y, z = target_p.x, target_p.y, target_p.z conditions = [ x <= BOUNDS_LEFTWALL, x >= BOUNDS_RIGHTWALL, y <= BOUNDS_BACKWALL, y >= BOUNDS_FRONTWALL, z <= BOUNDS_FLOOR, z >= 0.15 ] print("Target Position: %0.4f, %0.4f, %0.4f" % (x, y, z)) for condition in conditions: if not condition: return self.move_to_target(joint_goal) rospy.sleep(0.15)
def save_state(self, current_dict, robot_name): rs = RobotState() rospy.loginfo(current_dict) rs.joint_state = JointState() rs.joint_state.name = current_dict.keys() rs.joint_state.position = current_dict.values() rospy.logwarn(rs) self._save(self._name, robot_name, rs)
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() moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state self.move_group.set_start_state(moveit_robot_state) #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]] 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) replan_value = 0 error_code = None (plan, fraction) = self.move_group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold 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") return plan, fraction, waypoints
def __init__(self, name, hand_or_arm="both"): self.__save = rospy.ServiceProxy( 'save_robot_state', SaveState) self.__name = name if name is None or name == '': raise ValueError("Cannot save with empty name.") if hand_or_arm == "arm": self.__commander = SrArmCommander() if not self.__commander.arm_found(): raise ValueError("'No arm found.'") elif hand_or_arm == 'hand': self.__commander = SrHandCommander() else: double_error = [] try: self.__hand_commander = SrHandCommander() except Exception as e: double_error.append(str(e)) self.__arm_commander = SrArmCommander() if not self.__arm_commander.arm_found(): double_error.append("'No arm found.'") if len(double_error) != 0: raise ValueError(" ".join(double_error)) self.__hand_or_arm = hand_or_arm rs = RobotState() current_dict = {} if self.__hand_or_arm == "both": current_dict = self.__arm_commander.get_robot_state_bounded() robot_name = self.__arm_commander.get_robot_name() elif self.__hand_or_arm == "arm": current_dict = self.__commander.get_current_state_bounded() robot_name = self.__commander.get_robot_name() elif self.__hand_or_arm == "hand": current_dict = self.__commander.get_current_state_bounded() robot_name = self.__commander.get_robot_name() else: rospy.logfatal("Unknown save type") exit(-1) rospy.loginfo(current_dict) rs.joint_state = JointState() rs.joint_state.name = current_dict.keys() rs.joint_state.position = current_dict.values() self.__save(self.__name, robot_name, rs)
def get_cartesian_plan(self, trans, z_offset, start_state, grasp): # set argument start state moveit_start_state = RobotState() moveit_start_state.joint_state = start_state self.arm.set_start_state(moveit_start_state) # set waypoints waypoints = [] self.target_pose.position.x = trans.transform.translation.x self.target_pose.position.y = trans.transform.translation.y self.target_pose.position.z = trans.transform.translation.z q = (trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w) (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q) pitch += pi / 2.0 tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) self.target_pose.orientation.x = tar_q[0] self.target_pose.orientation.y = tar_q[1] self.target_pose.orientation.z = tar_q[2] self.target_pose.orientation.w = tar_q[3] wpose = Pose() wpose.position = copy.deepcopy(self.target_pose.position) wpose.orientation = copy.deepcopy(self.target_pose.orientation) wpose.position.z = copy.deepcopy(self.target_pose.position.z + z_offset) waypoints.append(wpose) # plan plan = RobotTrajectory() counter = 0 while len(plan.joint_trajectory.points) == 0: (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold counter += 1 self.arm.set_planning_time(self.planning_limitation_time + counter * 5.0) if counter > 1: return (False, start_state) self.arm.set_planning_time(self.planning_limitation_time) rospy.loginfo("!! Got a cartesian plan !!") # publish the plan pub_msg = HandringPlan() pub_msg.grasp = grasp pub_msg.trajectory = plan self.hp_pub.publish(pub_msg) self.arm.clear_pose_targets() # return goal state from generated trajectory goal_state = JointState() goal_state.header = Header() goal_state.header.stamp = rospy.Time.now() goal_state.name = plan.joint_trajectory.joint_names[:] goal_state.position = plan.joint_trajectory.points[-1].positions[:] return (True, goal_state)
def plan_trajectory(move_group, destination_pose, start_joint_angles): current_joint_state = JointState() current_joint_state.name = joint_names current_joint_state.position = start_joint_angles moveit_robot_state = RobotState() moveit_robot_state.joint_state = current_joint_state move_group.set_start_state(moveit_robot_state) move_group.set_pose_target(destination_pose) return planCompat(move_group.plan())
def srvPlanMovementCallback(data): group_name = data.group if group_name in groups_names: if data.rand_target: target_position = groups[group_name].get_random_joint_values() else: target_position = data.target_pos try: groups[group_name].set_joint_value_target(target_position) except: rospy.logwarn(rospy.get_caller_id() + ' Incorrect target positions') return PlanMovementResponse(3, [], [], []) rospy.loginfo(rospy.get_caller_id() + " Moving group '" + group_name + "' to " + str(data.target_pos)) joint_state = JointState() joint_state.header.stamp = rospy.Time.now() joint_state.header.stamp.secs += 1 joint_state.name = groups[group_name].get_active_joints() if data.rand_start: start_position = groups[group_name].get_random_joint_values() else: if data.current_start: start_position = groups[group_name].get_current_joint_values() else: start_position = data.start_pos joint_state.position = start_position moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state groups[group_name].set_start_state(moveit_robot_state) plan = groups[group_name].plan() if len(plan.joint_trajectory.points) > 0: rospy.loginfo(rospy.get_caller_id() + ' Correct plan') if data.execute: groups[group_name].execute(plan, wait=data.wait) if data.ret_plan: traj = planToNumpy(plan, data.ret_fps) plans = [] for joint in traj['data']: plans.append(Trajectory(joint, traj['data'][joint])) return PlanMovementResponse(0, start_position, target_position, plans) else: return PlanMovementResponse(0, [], [], []) else: rospy.logwarn(rospy.get_caller_id() + ' Could not plan') return PlanMovementResponse(2, [], [], []) else: rospy.logwarn(rospy.get_caller_id() + ' Incorrect group name: ' + group_name) return PlanMovementResponse(1, [], [], [])
def compute_fk_client(): try: header = Header() header.stamp = rospy.Time.now() header.frame_id = self.prefix + '/base' rs = RobotState() rs.joint_state = joint_state res = self.compute_fk(header, links, rs) return res.pose_stamped except rospy.ServiceException, e: print "Service call failed: %s" % e # in case of troubles return 0 pose stamped return []
def talker(): display_robot_state_publisher = rospy.Publisher( '/move_group/fake_controller_joint_states', JointState, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) print "============ Starting setup" robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander("arm") print "============ Robot Groups: %s" % robot.get_group_names() print "============ End effector: %s" % group.get_end_effector_link() active_joints = group.get_active_joints() print "============ Active joints: ", active_joints joints_values_str = group.get_current_joint_values() print "============ Joint values: ", joints_values_str group.clear_pose_targets() print "============ Set start state..." i = 0 while i != len(active_joints): a = float(input("Vvedite znachenie %s: " % active_joints[i])) joints_values_str[i] = a i = i + 1 start_joint_state = JointState() start_joint_state.header.stamp = rospy.Time.now() start_joint_state.name = active_joints start_joint_state.position = joints_values_str moveit_robot_state = RobotState() moveit_robot_state.joint_state = start_joint_state group.set_start_state(moveit_robot_state) #group.set_start_state_to_current_state() display_robot_state_publisher.publish(start_joint_state) print "============ Display start state..." while not rospy.is_shutdown(): #display_robot_state = moveit_msgs.msg.DisplayRobotState() #display_robot_state.state = moveit_robot_state #Object_color = ObjectColor() #Object_color.id = active_joints #Object_color.color = ColorRGBA(250, 118, 0, 1) #display_robot_state.highlight_links = [250, 118, 0, 1] #display_robot_state_publisher.publish(start_joint_state) #group.set_start_state(moveit_robot_state) rate.sleep()
def _make_request_msg(root_link, tip_link, target_pose, robot_object, joints): """ This method generates an ik request message for the kdl_ik_service. The message is of the type moveit_msgs/PositionIKRequest and contains all information contained in the parameter. :param root_link: The first link of the chain of joints to be altered :param tip_link: The last link of the chain of joints to be altered :param target_pose: A list of two lists, the first is the pose in world coordinate frame the second is the orientation as quanternion in world coordinate frame :param robot_object: The robot for which the ik should be generated :param joints: A list of joint names between the root_link and tip_link that should be altered. :return: A moveit_msgs/PositionIKRequest message containing all the information from the parameter """ pose_sta = PoseStamped() pose_sta.header.frame_id = root_link pose_sta.header.stamp = rospy.Time.now() tar_pose = target_pose[0] tar_rotation = target_pose[1] pose = Pose() pose.position.x = tar_pose[0] pose.position.y = tar_pose[1] pose.position.z = tar_pose[2] pose.orientation.x = tar_rotation[0] pose.orientation.y = tar_rotation[1] pose.orientation.z = tar_rotation[2] pose.orientation.w = tar_rotation[3] pose_sta.pose = pose robot_state = RobotState() joint_state = JointState() names, poses = _get_position_for_all_joints(robot_object) joint_state.name = joints joint_state.position = _get_position_for_joints(robot_object, joints) joint_state.velocity = [0.0 for x in range(len(joints))] joint_state.effort = [0.0 for x in range(len(joints))] #joint_state.name = names #joint_state.position = poses robot_state.joint_state = joint_state msg_request = PositionIKRequest() #msg_request.group_name = "arm" msg_request.ik_link_name = tip_link msg_request.pose_stamped = pose_sta msg_request.avoid_collisions = False msg_request.robot_state = robot_state #msg_request.timeout = rospy.Duration(secs=10000) #msg_request.attempts = 1000 return msg_request
def _convertRobotState(self, context, jointStates): si = context.getSceneInformation() robotInfo = si.getRobotInfo() config = robotInfo.configuration robotStateMsg = RobotState() jointStateMsg = JointState( header=Header(stamp=rospy.Time.now(), frame_id='/world')) for jointName in jointStates: jointStateMsg.name.append(jointName) jointStateMsg.position.append(config[jointName]) # jointStateMsg.velocity.append(0.0) # jointStateMsg.effort.append(0.0) robotStateMsg.joint_state = jointStateMsg return robotStateMsg
def joint_state_to_robot_state(self, joint_state): """ Create a RobotState message from a JointState message or a dictionary mapping joint names to their values @param joint_state: JointState message or dictionary with the following format {'joint_name': joint_value} """ if isinstance(joint_state, dict): joint_state_message = JointState() joint_state_message.name = joint_state.keys() joint_state_message.position = joint_state.values() else: joint_state_message = joint_state moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state_message return moveit_robot_state
def set_start_state(self, group_name, joint_names, joint_positions): group = self.get_group(group_name) joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = joint_names joint_state.position = joint_positions moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state)
def cartesian_move(group, waypoints, rate): ready_state = RobotState() joint_state = JointState() joint_state.name = group.get_active_joints() joint_state.position = group.get_current_joint_values() ready_state.joint_state = joint_state # compute a path consised of straight line segments from a series of waypoints (plan, fraction) = group.compute_cartesian_path(waypoints, 0.0001, 0) print(len(plan.joint_trajectory.points)) print('path fraction: {}'.format(fraction)) # arg ref_state_in is the start state of robot new_plan = group.retime_trajectory(ready_state, plan, 0.005) group.execute(new_plan) group.stop()
def get_plan(self, trans, z_offset, start_state, grasp): # Set argument start state moveit_start_state = RobotState() moveit_start_state.joint_state = start_state self.arm.set_start_state(moveit_start_state) # Calculate goal pose self.target_pose.position.x = trans.transform.translation.x self.target_pose.position.y = trans.transform.translation.y self.target_pose.position.z = trans.transform.translation.z + z_offset q = (trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w) (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q) pitch += pi / 2.0 tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) self.target_pose.orientation.x = tar_q[0] self.target_pose.orientation.y = tar_q[1] self.target_pose.orientation.z = tar_q[2] self.target_pose.orientation.w = tar_q[3] self.arm.set_pose_target(self.target_pose) # plan plan = RobotTrajectory() counter = 0 while len(plan.joint_trajectory.points) == 0: plan = self.arm.plan() counter += 1 self.arm.set_planning_time(self.planning_limitation_time + counter * 5.0) if counter > 1: return (False, start_state) self.arm.set_planning_time(self.planning_limitation_time) rospy.loginfo("!! Got a plan !!") # publish the plan pub_msg = HandringPlan() pub_msg.grasp = grasp print("---------debug--------{}".format( len(plan.joint_trajectory.points))) pub_msg.trajectory = plan self.hp_pub.publish(pub_msg) self.arm.clear_pose_targets() # return goal state from generated trajectory goal_state = JointState() goal_state.header = Header() goal_state.header.stamp = rospy.Time.now() goal_state.name = plan.joint_trajectory.joint_names[:] goal_state.position = plan.joint_trajectory.points[-1].positions[:] return (True, goal_state)
def execute(self): rospy.loginfo("Start Task") # Get latest task plan plan = self.task_q[0] for points in plan.trajectory.joint_trajectory.points: tfs = points.time_from_start.to_sec() tfs /= self.exe_speed_rate points.time_from_start = rospy.Duration(tfs) scaled_vel = [] scaled_acc = [] for vel in points.velocities: scaled_vel.append(vel * self.exe_speed_rate) points.velocities = tuple(scaled_vel) for acc in points.accelerations: scaled_acc.append(acc * self.exe_speed_rate * self.exe_speed_rate) points.accelerations = tuple(scaled_acc) # Display the Trajectory start_state = JointState() start_state.header = Header() start_state.header.stamp = rospy.Time.now() start_state.name = plan.trajectory.joint_trajectory.joint_names[:] start_state.position = plan.trajectory.joint_trajectory.points[ -1].positions[:] moveit_start_state = RobotState() moveit_start_state.joint_state = start_state pub_display_msg = DisplayTrajectory() pub_display_msg.model_id = "vs087" pub_display_msg.trajectory.append(plan.trajectory) pub_display_msg.trajectory_start = moveit_start_state self.display_hp_pub.publish(pub_display_msg) # Send Action and Wait result goal = ExecutePlanAction().action_goal.goal goal.planning_time = plan.planning_time goal.start_state = plan.start_state # goal.start_state.joint_state.header.stamp = rospy.Time.now() goal.trajectory = plan.trajectory # rospy.logwarn(goal) self.client.send_goal(goal) self.client.wait_for_result() # Update the task queue self.task_q.pop(0) rospy.loginfo("End Task")
def get_plan(end, start=None, max_planning_time=100): global group if not start: start = group.get_current_joint_values() joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] joint_state.position = start moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) group.set_joint_value_target(end) group.set_planning_time(max_planning_time) return group.plan()
def get_robot_state(self): # pose = pose_from_trans(self.get_world_pose(BASE_FRAME)) # TODO: could get this transform directly # transform = Transform(Vector3(*point_from_pose(pose)), Quaternion(*quat_from_pose(pose))) # transform = self.get_transform() # if transform is None: # return None state = RobotState() state.joint_state = self.joint_state # state.multi_dof_joint_state.header.frame_id = '/base_footprint' # state.multi_dof_joint_state.header.stamp = rospy.Time(0) # state.multi_dof_joint_state.joints = ['world_joint'] # state.multi_dof_joint_state.transforms = [transform] # 'world_joint' # http://cram-system.org/tutorials/intermediate/moveit state.attached_collision_objects = [] state.is_diff = False # rostopic info /joint_states return state
def plan_cartesian_path(self, max_tries, waypoints, start_joints): # Call the planner move_group = self.move_group #------------------------------------------------------------------------------------------------------------ #- inforamtions on move_group http://docs.ros.org/jade/api/moveit_commander/html/move__group_8py_source.html #- http://docs.ros.org/melodic/api/moveit_msgs/html/index-msg.html #------------------------------------------------------------------------------------------------------------ #-- Check for robot stability if(n.get_learning_mode()): print "Make sure that Niryo is not in learning mode and in a safe position close to where you want to print" raw_input("Then press enter..") #-- Saving start state tab_joints = [start_joints[0], start_joints[1],start_joints[2],start_joints[3],start_joints[4],start_joints[5]] #-- Sending start state joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = ['joint_1', 'joint_2','joint_3','joint_4','joint_5','joint_6'] joint_state.position = tab_joints initial_state = RbState() initial_state.joint_state = joint_state move_group.set_start_state(initial_state) #-- Parameters fraction = 0.0 tries = 0 max_tries = max_tries #maximum tries allowed eef_step = 1.0 #eef_step at 1.0 considering gcode is already an interpolation velocity = 0.06 #velocity scaling factor applied to max velocity #print "\n --- Computing parameters ---" #print "| Max tries authorized : %2d \n| Eef step : %.4f \n| Velocity : #%3d %%" %(max_tries,eef_step,velocity*100) #print " ------------------------------\n" #-- Call cartesian service try: moveit_cartesian_path = rospy.ServiceProxy('compute_cartesian_path', GetCartesianPath) except rospy.ServiceException,e: print("Service call failed:",e) return(None)
def check_config(config, state_validity_checker, limb_name): """ Check the validity of a configuration :param : The configurations to check against. :param state_validity_checker: The validity checker :param space: The space to check for :param limb_name: The name of the limb. :return: The validty of the configurations. """ # Construct a robotstate object from a dictionary rs = RobotState() js = JointState() js.name = get_joint_names('right') js.position = [config[joint] for joint in js.name] rs.joint_state = js result = state_validity_checker.getStateValidity(rs, group_name=limb_name + '_arm') return result.valid
def plan_trajectory(move_group, destination_pose, start_joint_angles): current_joint_state = JointState() current_joint_state.name = joint_names current_joint_state.position = start_joint_angles moveit_robot_state = RobotState() moveit_robot_state.joint_state = current_joint_state move_group.set_start_state(moveit_robot_state) move_group.set_pose_target(destination_pose) plan = move_group.go(wait=True) if not plan: exception_str = """ Trajectory could not be planned for a destination of {} with starting joint angles {}. Please make sure target and destination are reachable by the robot. """.format(destination_pose, destination_pose) raise Exception(exception_str) return move_group.plan()
def srvOffsetMovementCallback(data): group_name = data.group if group_name in groups_names: start_position = groups[group_name].get_current_joint_values() target_position = [0] * len(start_position) offset_position = data.offset for i in range(len(start_position)): target_position[i] = start_position[i] + offset_position[i] try: groups[group_name].set_joint_value_target(target_position) except: rospy.logwarn(rospy.get_caller_id() + ' Incorrect target positions') return OffsetMovementResponse(3, start_position, target_position) rospy.loginfo(rospy.get_caller_id() + " Moving group '" + group_name + "' to " + str(target_position)) joint_state = JointState() joint_state.header.stamp = rospy.Time.now() joint_state.header.stamp.secs += 1 joint_state.name = groups[group_name].get_active_joints() joint_state.position = start_position moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state groups[group_name].set_start_state(moveit_robot_state) plan = groups[group_name].plan() if len(plan.joint_trajectory.points) > 0: rospy.loginfo(rospy.get_caller_id() + ' Correct plan') if data.execute: groups[group_name].execute(plan, wait=data.wait) return OffsetMovementResponse(0, start_position, target_position) else: rospy.logwarn(rospy.get_caller_id() + ' Could not plan') return OffsetMovementResponse(2, start_position, target_position) else: rospy.logwarn(rospy.get_caller_id() + ' Incorrect group name: ' + group_name) return OffsetMovementResponse(1, [], [])
def __save_out(self): rs = RobotState() current_dict = {} if self.__hand_or_arm == "both": current_dict = self.__arm_commander.get_robot_state_bounded() robot_name = self.__arm_commander.get_robot_name() elif self.__hand_or_arm == "arm": current_dict = self.__commander.get_current_state_bounded() robot_name = self.__commander.get_robot_name() elif self.__hand_or_arm == "hand": current_dict = self.__commander.get_current_state_bounded() robot_name = self.__commander.get_robot_name() else: rospy.logfatal("Unknown save type") exit(-1) rospy.loginfo(current_dict) rs.joint_state = JointState() rs.joint_state.name = current_dict.keys() rs.joint_state.position = current_dict.values() self.__save(self.__name, robot_name, rs)
def loop(self): hz = get_param("rate", 10) # 10hz r = rospy.Rate(hz) delta = get_param("delta", 0.0) msg = JointState() msgDisplayRobotState = DisplayRobotState() msgRobotState = RobotState() # Publish Joint States while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() if delta > 0: self.update(delta) # Initialize msg.position, msg.velocity, and msg.effort. has_position = len(self.dependent_joints.items()) > 0 has_velocity = False has_effort = False for (name,joint) in self.free_joints.items(): if not has_position and 'position' in joint: has_position = True if not has_velocity and 'velocity' in joint: has_velocity = True if not has_effort and 'effort' in joint: has_effort = True num_joints = (len(self.free_joints.items()) + len(self.dependent_joints.items())) if has_position: msg.position = num_joints * [0.0] if has_velocity: msg.velocity = num_joints * [0.0] if has_effort: msg.effort = num_joints * [0.0] for i, name in enumerate(self.joint_list): msg.name.append(str(name)) joint = None # Add Free Joint if name in self.free_joints: joint = self.free_joints[name] factor = 1 offset = 0 # Add Dependent Joint elif name in self.dependent_joints: param = self.dependent_joints[name] parent = param['parent'] joint = self.free_joints[parent] factor = param.get('factor', 1) offset = param.get('offset', 0) if has_position and 'position' in joint: pos = self.joint_positions[ name ] msg.position[i] = pos if has_velocity and 'velocity' in joint: msg.velocity[i] = joint['velocity'] * factor if has_effort and 'effort' in joint: msg.effort[i] = joint['effort'] msgRobotState.joint_state = msg msgDisplayRobotState.state = msgRobotState self.pub.publish( msgDisplayRobotState ) r.sleep()
current_joint_states = rospy.wait_for_message('/joint_states', JointState) current_joint_states_modif = copy.deepcopy(current_joint_states) current_joint_states_modif.position = [2.0] * len(current_joint_states_modif.position) rs_pub = rospy.Publisher('fk_test_robot_state', DisplayRobotState) rospy.sleep(0.3) # let it initialize... ik = InverseKinematics() correct_js = copy.deepcopy(current_joint_states) result = fk.getFK('arm_right_7_link', current_joint_states.name, correct_js.position, 'base_link') rospy.loginfo("Result of current robot pose FK is: " + str(result)) rs = RobotState() rs.joint_state = correct_js drs = DisplayRobotState() drs.state = rs rs_pub.publish(drs) rospy.loginfo("Published current robot state") rospy.sleep(2.0) c = Constraints() jc = JointConstraint() jc.joint_name = 'arm_right_1_link' jc.position = 0.0 jc.tolerance_above = 0.00001 jc.tolerance_below = 0.00001 jc.weight = 1.0 c.joint_constraints.append(jc)
for i in range(10): rospy.loginfo("Random pose: " + str(i)) random_js = copy.deepcopy(current_joint_states) positions_list = [] for id, item in enumerate(current_joint_states_modif.position): positions_list.append(random.random() * 3.14) random_js.position = positions_list # c = Constraints() # jc = JointConstraint() # jc.joint_name = 'arm_right_1_link' # c.joint_constraints.append() result = fk.getFK('arm_right_7_link', current_joint_states.name, random_js.position, 'base_link') rospy.loginfo("Result of a disastrous robot pose is: " + str(result)) rs = RobotState() rs.joint_state = random_js drs = DisplayRobotState() drs.state = rs rs_pub.publish(drs) rospy.loginfo("Published robot state") rospy.loginfo("Asking for IK with collision avoidance for the pose given") print "Result is:" print result print "And result.pose_stamped is:" print result.pose_stamped print "with result.pose_stamped[0]" print result.pose_stamped[0] resultik = ik.getIK("right_arm", "right_arm_7_link", result.pose_stamped[0], True, 0, rs) rospy.loginfo(str(resultik)) rospy.sleep(1.0)