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 joint_to_robot_state(joint_state, robot_state=None): """ Converts joint state to robot state filling missing joints from the current or given robot state. @joint_state: JointState to convert @robot_state: Optional RobotState that is used to fill missing values, if None current state is used. """ # Get robot state if empty if robot_state is None: robot_state = get_robot_state() # Convert joint state to dictionary js_dict = dict( (x, y) for x, y in zip(joint_state.name, joint_state.position)) # Fill joint_state values into robot_state rs = RobotState() for i, k in enumerate(robot_state.joint_state.name): if k in js_dict: rs.joint_state.name.append(k) rs.joint_state.position.append(js_dict[k]) rs.joint_state.velocity.append(0) else: rs.joint_state.name.append(k) rs.joint_state.position.append(robot_state.joint_state.position[i]) rs.joint_state.velocity.append(robot_state.joint_state.velocity[i]) rs.multi_dof_joint_state = robot_state.multi_dof_joint_state rs.attached_collision_objects = robot_state.attached_collision_objects return rs
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 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 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 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 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 __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 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 move_arm(): # plan to a random location group = moveit_commander.MoveGroupCommander("arm") robot = RobotCommander() a = robot.arm a.set_start_state(RobotState()) p = a.plan([0, 0]) print "Solution:" print p a.execute(p) rospy.sleep(10) a.set_start_state(RobotState()) p = a.plan([1.57, -1.57]) print "Solution:" print p a.execute(p) rospy.sleep(10) a.set_start_state(RobotState()) p = a.plan([3.14, 3.14]) print "Solution:" print p a.execute(p) rospy.sleep(10) moveit_commander.roscpp_shutdown() print "Finishing"
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 __init__(self): self.group = MoveGroupInterface("arm", "base_link", plan_only=True) self.gripper = MoveGroupInterface("gripper", "base_link", plan_only=True) self.virtual_arm_state = RobotState() self.virtual_gripper_state = RobotState() # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(5) print "============ Starting planning"
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 set_global_defaults( self, group ): """ Initializes the proxy with default parameters to use with connections @param group : string move group name """ # Default planning parameters for new connections num_planning_attempts=5 allowed_planning_time=10.0 plan_only=True planner_id='RRTConnectkConfigDefault' # Set the default plan request data plan_request = MotionPlanRequest() plan_request.start_state.is_diff = True # Flags start state as empty to use current state on server plan_request.num_planning_attempts = num_planning_attempts plan_request.allowed_planning_time = allowed_planning_time plan_request.planner_id = planner_id plan_request.group_name = group #@TODO - retrieve from ROS parameters ProxyMoveItClient._motion_plan_requests[group] = plan_request ProxyMoveItClient._default_motion_plan_requests[group] = copy.deepcopy(plan_request) planning_options = PlanningOptions() planning_options.plan_only = plan_only planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server #@TODO - retrieve from ROS parameters ProxyMoveItClient._planning_options[group] = planning_options ProxyMoveItClient._default_planning_options[group] = copy.deepcopy(planning_options) # Set up the default joint constraints joint_constraints = {} for name in ProxyMoveItClient._joint_names[group]: joint_constraints[name] = JointConstraint(joint_name=name, tolerance_above=0.05, tolerance_below=0.05,weight=1.0) #@TODO - retrieve from ROS parameters ProxyMoveItClient._joint_constraints[group] = joint_constraints ProxyMoveItClient._default_joint_constraints[group] = copy.deepcopy(joint_constraints) # @TODO - add real constraints ProxyMoveItClient._position_constraints[group] = [] ProxyMoveItClient._default_position_constraints[group] = [] ProxyMoveItClient._orientation_constraints[group] = [] ProxyMoveItClient._default_orientation_constraints[group] = [] ProxyMoveItClient._visibility_constraints[group] = [] ProxyMoveItClient._default_visibility_constraints[group] = [] ProxyMoveItClient._robot_states[group] = RobotState() ProxyMoveItClient._default_robot_states[group] = RobotState()
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 _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 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 _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 enforce_bounds(self, robot_state_msg): """ Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """ s = RobotState() c_str = self._g.enforce_bounds( conversions.msg_to_string(robot_state_msg)) conversions.msg_from_string(s, c_str) return s
def benchmark(self): for test_id, test in enumerate(self._annotations["tests"]): marker_position_1 = test["start_xyz"] marker_position_2 = test["goal_xyz"] self._add_markers(marker_position_1, "Start test \n sequence", marker_position_2, "Goal") # Start planning in a given joint position joints = test["start_joints"] current = RobotState() current.joint_state.name = self.robot.get_current_state( ).joint_state.name current_joints = list( self.robot.get_current_state().joint_state.position) current_joints[0:6] = joints current.joint_state.position = current_joints self.group.set_start_state(current) joints = test["goal_joints"] for planner in self.planners: if planner == "stomp": planner = "STOMP" elif planner == "sbpl": planner = "AnytimeD*" self.planner_id = planner self.group.set_planner_id(planner) self._plan_joints( joints, self._annotations["name"] + "-test_" + str(test_id)) return self.planner_data
def checkTrajectoryValidity(self, robot_trajectory, groups=[]): """Given a robot trajectory, deduce it's groups and check it's validity on each point of the traj returns True if valid, False otherwise It's considered not valid if any point is not valid""" #r = RobotTrajectory() init_time = time.time() if len(groups) > 0: groups_to_check = groups else: groups_to_check = [ 'both_arms_torso' ] # Automagic group deduction... giving a group that includes everything for traj_point in robot_trajectory.joint_trajectory.points: rs = RobotState() rs.joint_state.name = robot_trajectory.joint_trajectory.joint_names rs.joint_state.position = traj_point.positions for group in groups_to_check: result = self.sv.getStateValidity(rs, group) #, constraints) if not result.valid: # if one point is not valid, the trajectory is not valid rospy.logerr( "Trajectory is not valid at point (RobotState):" + str(rs) + "with result of StateValidity: " + str(result)) rospy.logerr( "published in /robot_collision_state the conflicting state" ) drs = DisplayRobotState() drs.state = rs self.robot_state_collision_pub.publish(drs) return False fin_time = time.time() rospy.logwarn("Trajectory validity of " + str(len(robot_trajectory.joint_trajectory.points)) + " points took " + str(fin_time - init_time)) return True
def get_forward_kinematic(joints): try: rospy.wait_for_service('compute_fk', 2) except (rospy.ServiceException, rospy.ROSException) as e: rospy.logerr("Service call failed:", e) return None try: moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK) fk_link = ['base_link', 'tool_link'] joint_names = ['joint_1','joint_2','joint_3','joint_4','joint_5','joint_6'] header = Header(0,rospy.Time.now(),"/world") rs = RobotState() rs.joint_state.name = joint_names rs.joint_state.position = joints response = moveit_fk(header, fk_link, rs) except rospy.ServiceException as e: rospy.logerr("Service call failed:", e) return(None) quaternion=[response.pose_stamped[1].pose.orientation.x, response.pose_stamped[1].pose.orientation.y, response.pose_stamped[1].pose.orientation.z, response.pose_stamped[1].pose.orientation.w] rpy = get_rpy_from_quaternion(quaternion) quaternion = Position.Quaternion(round(quaternion[0],3), round(quaternion[1],3), round(quaternion[2],3), round(quaternion[3],3)) point = Position.Point(round(response.pose_stamped[1].pose.position.x,3), round(response.pose_stamped[1].pose.position.y,3), round(response.pose_stamped[1].pose.position.z,3)) rpy=Position.RPY(round(rpy[0],3),round(rpy[1],3),round(rpy[2],3)) rospy.loginfo("kinematic forward has been calculated ") return(point, rpy, quaternion)
def test_get_current_state(self): expected_state = RobotState() expected_state.joint_state.header.frame_id = "base_link" expected_state.multi_dof_joint_state.header.frame_id = "base_link" expected_state.joint_state.name = self.JOINT_NAMES expected_state.joint_state.position = [0] * 6 self.assertEqual(self.group.get_current_state(), expected_state)
def plan_and_execute(self, userdata): ### DEBUG PERCEPTION ONLY #sss.wait_for_input() #userdata.new_box = True #return True ### END DEBUG # print "The pose reference frame is",self.mgc.get_pose_reference_frame() # self.mgc.set_pose_reference_frame("camera_rgb_optical_frame") # print "The pose reference frame is",self.mgc.get_pose_reference_frame() start_plan = rospy.Time.now() ### Set next (virtual) start state start_state = RobotState() (pre_grasp_config, error_code) = sss.compose_trajectory("arm","pre_grasp") if error_code != 0: rospy.logerr("unable to parse pre_grasp configuration") return False start_state.joint_state.name = pre_grasp_config.joint_names start_state.joint_state.position = pre_grasp_config.points[0].positions start_state.is_diff = True # self.mgc.set_start_state(start_state) self.mgc.set_start_state_to_current_state() ### Plan Approach approach_pose_offset = PoseStamped() approach_pose_offset.header.frame_id = "current_box" approach_pose_offset.header.stamp = rospy.Time(0) # approach_pose_offset.pose.position.x -= 0.1 approach_pose_offset.pose.position.z += 0.05 #quat = tf.transformations.quaternion_from_euler(0, 1.57, 1.57) #approach_pose_offset.pose.orientation.x = quat[0] #approach_pose_offset.pose.orientation.y = quat[1] #approach_pose_offset.pose.orientation.z = quat[2] #approach_pose_offset.pose.orientation.w = quat[3] try: approach_pose = self.listener.transformPose("table", approach_pose_offset) approach_pose.pose.position.x += -0.01 # approach_pose = self.listener.transformPose("camera_rgb_optical_frame", approach_pose_offset) except Exception, e: rospy.logerr("could not transform pose. Exception: %s", str(e)) return False
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None): solutions = [] for pose_num, eef_pose in enumerate(eef_poses): if eef_pose.header.frame_id.strip('/') != self._world.strip('/'): raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world)) pose = pose_to_list(eef_pose) resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1], [seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)] for joint in self.joint_names()] if len(seeds)>0 else None) if resp is None: rs = None else: rs = RobotState() rs.is_diff = False rs.joint_state.name = self.joint_names() rs.joint_state.position = resp solutions.append(rs) return solutions
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()
rospy.loginfo("Getting a current joint states message") 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)
def get_current_state(self): """ Get a RobotState message describing the current state of the robot""" s = RobotState() s.deserialize(self._r.get_current_state()) return s
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)