Python RobotCommander.get_group_names方法代码示例

# 需要导入模块: from moveit_commander import RobotCommander [as 别名]
# 或者: from moveit_commander.RobotCommander import get_group_names [as 别名]

#.........这里部分代码省略.........
            res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
        return (MoveGroupInfoLevel.INFO, "\n".join(res))
        
    def command_current(self, g):
        res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
        if len(g.get_end_effector_link()) > 0:
            res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
            res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
        return (MoveGroupInfoLevel.INFO, res)

    def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
        if g.has_end_effector_link():
            try:
                g.shift_pose_target(dimension_index, factor * offset)
                if g.go():
                    return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
                else:
                    return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
            except MoveItCommanderException as e:
                return (MoveGroupInfoLevel.WARN, str(e))
            except:
                return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
        else:
            return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)

    def resolve_command_alias(self, cmdorig):
        cmd = cmdorig.lower()
        if cmd == "which":
            return "id"
        if cmd == "groups":
            return "use"
        return cmdorig

    def get_help(self):
        res = []
        res.append("Known commands:")
        res.append("  help                show this screen")
        res.append("  id|which            display the name of the group that is operated on")
        res.append("  load [<file>]       load a set of interpreted commands from a file")
        res.append("  save [<file>]       save the currently known variables as a set of commands")
        res.append("  use <name>          switch to using the group named <name> (and load it if necessary)")
        res.append("  use|groups          show the group names that are already loaded")
        res.append("  vars                display the names of the known states")
        res.append("  show                display the names and values of the known states")
        res.append("  show <name>         display the value of a state")
        res.append("  record <name>       record the current joint values under the name <name>")
        res.append("  delete <name>       forget the joint values under the name <name>")
        res.append("  current             show the current state of the active group")
        res.append("  constrain <name>    use the constraint <name> as a path constraint")
        res.append("  constrain           clear path constraints")
        res.append("  eef                 print the name of the end effector attached to the current group")
        res.append("  go <name>           plan and execute a motion to the state <name>")
        res.append("  go <dir> <dx>|      plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
        res.append("  go rand             plan and execute a motion to a random state")
        res.append("  plan <name>         plan a motion to the state <name>")
        res.append("  execute             execute a previously computed motion plan")
        res.append("  rotate <x> <y> <z>  plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
        res.append("  tolerance           show the tolerance for reaching the goal region")
        res.append("  tolerance <val>     set the tolerance for reaching the goal region")
        res.append("  wait <dt>           sleep for <dt> seconds")
        res.append("  x = y               assign the value of y to x")
        res.append("  x[idx] = val        assign a value to dimension idx of x")
        res.append("  x = [v1 v2...]      assign a vector of values to x")
        res.append("  trace <on|off>      enable/disable replanning or looking around")
        res.append("  ground              add a ground plane to the planning scene")
        res.append("  allow replanning <true|false>    enable/disable replanning")
        res.append("  allow looking <true|false>       enable/disable looking around")
        return "\n".join(res)

    def get_keywords(self):
        known_vars = []
        known_constr = []
        if self.get_active_group() != None:
            known_vars = self.get_active_group().get_remembered_joint_values().keys()
            known_constr = self.get_active_group().get_known_constraints()
        groups = self._robot.get_group_names()
        return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars,
                'help':[],
                'record':known_vars,
                'show':known_vars,
                'wait':[],
                'delete':known_vars,
                'database': [],
                'current':[],
                'use':groups,
                'load':[],
                'save':[],
                'pick':[],
                'place':[],
                'plan':known_vars,
                'allow':['replanning', 'looking'],
                'constrain':known_constr,
                'vars':[],
                'joints':[],
                'tolerance':[],
                'time':[],
                'eef':[],
                'execute':[],
                'ground':[],
                'id':[]}

# 需要导入模块: from moveit_commander import RobotCommander [as 别名]
# 或者: from moveit_commander.RobotCommander import get_group_names [as 别名]
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
import tf

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("arm")
    print "============ Robot Groups:"
    print robot.get_group_names()
    print "============ Robot Links for arm:"
    print robot.get_link_names("arm")
    print "============ Robot Links for gripper:"
    print robot.get_link_names("gripper")
    print right_arm.get_end_effector_link()
    print "============ Printing robot state"
    #print robot.get_current_state()
    print "============"

    
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("pole")
    scene.remove_world_object("table")

# 需要导入模块: from moveit_commander import RobotCommander [as 别名]
# 或者: from moveit_commander.RobotCommander import get_group_names [as 别名]
 p.pose.position.x = 0.7
 p.pose.position.y = 0.7
 p.pose.position.z = 0.0
 # add the cup
 scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
 
 # modify the pose
 p.pose.position.x = 0.72
 p.pose.position.z = 0.05
 # add the pen
 scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
 rospy.sleep(1)
  
 # print the existing groups 
 robot = RobotCommander()
 print "Available groups: ",robot.get_group_names()
 
 # setup the arm group and its planner
 arm = MoveGroupCommander("arm")
 arm.set_start_state_to_current_state()
 arm.set_planner_id("RRTstarkConfigDefault") 
 arm.set_planning_time(5.0)
 arm.detach_object("pen")
 
 # set the arm to a safe target
 arm.set_named_target("gamma")
 # plan and execute the motion 
 arm.go()
 
 # setup the hand group and its planner
 hand = MoveGroupCommander("hand")