Smach planning

Plan to pose


#!/usr/bin/env python
import roslib; roslib.load_manifest('clopema_smach')
import rospy, smach, smach_ros, math, copy, tf, PyKDL, os, shutil, numpy
from tf.transformations import quaternion_from_euler, quaternion_about_axis
from tf_conversions import posemath
from clopema_smach import *
from geometry_msgs.msg import *
from smach import State
from clopema_planning_actions.msg import MA1400JointState

def main():
    rospy.init_node('sm_test')

    pose = PoseStamped()
    pose.header.frame_id = 'base_link'
    pose.pose.position.x = 0.5
    pose.pose.position.y = -0.8
    pose.pose.position.z = 1.5
    pose.pose.orientation = Quaternion(*quaternion_from_euler(math.pi, 0, math.pi))

    sm = smach.Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
    sm_plan = gensm_plan_vis_exec(Plan1ToPoseState(), input_keys=['goal', 'ik_link'])

    sm.userdata.goal = pose
    sm.userdata.ik_link = 'r2_tip_link' #r2_ee

    with sm:
        smach.Sequence.add('GOTO', sm_plan, transitions={'aborted':'POWER_OFF'})
        smach.Sequence.add('POWER_OFF', SetServoPowerOffState())

    sis = smach_ros.IntrospectionServer('sm_test', sm, '/SM_ROOT')
    sis.start()
    os.system('clear')
    outcome = sm.execute()
    sis.stop()

if __name__ == '__main__':
    main()

Plan to poses

#!/usr/bin/env python
import roslib; roslib.load_manifest('clopema_smach')
import rospy, smach, smach_ros, math, copy, tf, PyKDL, os, shutil, numpy
from tf.transformations import quaternion_from_euler, quaternion_about_axis
from tf_conversions import posemath
from clopema_smach import *
from geometry_msgs.msg import *
from smach import State
from clopema_planning_actions.msg import MA1400JointState
from clopema_smach.utility_states import PoseBufferState

def main():
    rospy.init_node('sm_test')

    pose = Pose()
    pose.position.x = 0.5
    pose.position.y = -0.8
    pose.position.z = 1.5
    pose.orientation = Quaternion(*quaternion_from_euler(math.pi, 0, math.pi))

    poses = []
    poses.append(copy.deepcopy(pose))
    pose.position.z = 1.7
    poses.append(copy.deepcopy(pose))

    sm = smach.Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
    sm_plan = gensm_plan_vis_exec(Plan1ToPoseState(), input_keys=['goal', 'ik_link'])
    sm.userdata.poses = PoseArray()
    sm.userdata.poses.header.frame_id = 'base_link'
    sm.userdata.poses.poses = poses
    sm.userdata.ik_link = 'r2_tip_link' #r2_ee

    with sm:
        smach.Sequence.add('POSE_BUFFER', PoseBufferState(), transitions={'aborted':'POWER_OFF'})
        smach.Sequence.add('GOTO', sm_plan, transitions={'aborted':'POSE_BUFFER', 'succeeded':'POSE_BUFFER'},
                           remapping={'goal':'pose'})
        smach.Sequence.add('POWER_OFF', SetServoPowerOffState())

    sis = smach_ros.IntrospectionServer('sm_test', sm, '/SM_ROOT')
    sis.start()
    os.system('clear')
    outcome = sm.execute()
    sis.stop()

if __name__ == '__main__':
    main()

Move To Joint

#!/usr/bin/env python

import roslib; roslib.load_manifest('clopema_demos')
import rospy, sys, copy, os, math

from smach                        import Sequence
from clopema_motoros.srv          import *
from clopema_planning_actions.msg import MA1400JointState
from clopema_smach                import *

# Controls whether to start introspection server
start_sis = False

def main():
    rospy.init_node("move_arm_sm")

    sq = Sequence( outcomes = ['succeeded','aborted','preempted'], connector_outcome = 'succeeded')
    sm2 = gensm_plan_vis_exec(Plan1ToJointsState())
    sm2.userdata.goal = MA1400JointState()
    sm2.userdata.goal.s = -math.pi /2
    sm2.userdata.goal.l = 0
    sm2.userdata.goal.u = 0
    sm2.userdata.goal.r = 0
    sm2.userdata.goal.b = 0
    sm2.userdata.goal.t = 0
    sm2.userdata.robot = 1 

    with sq:
        Sequence.add("EXTA", sm2, transitions = {'aborted':'SOFF'})
        Sequence.add("SOFF", SetServoPowerOffState())

    # Create and start the introspection server
    if start_sis:
        sis = smach_ros.IntrospectionServer('introspection_server', sq, '/MOVE_HOME')
        sis.start()

    # Execute the state machine
    outcome = sq.execute()
    rospy.loginfo("State machine exited with outcome: " + outcome )

    # Wait for ctrl-c to stop the application
    if start_sis:
        sis.stop()

if __name__ == '__main__':
    main()


Move To Joints

#!/usr/bin/env python
import roslib; roslib.load_manifest('clopema_calibration')
import rospy, smach, smach_ros, math, copy, tf, PyKDL, os, shutil, numpy
from tf.transformations import quaternion_from_euler, quaternion_about_axis
from tf_conversions import posemath
from clopema_smach import *
from geometry_msgs.msg import *
from smach import State
from clopema_planning_actions.msg import MA1400JointState

goals = []

def pick_joints(ud):
    ud.goal = goals[ud.n]
    print goals[ud.n]
    return 'succeeded'

def to_rad(deg):
    return deg * math.pi / 180

def main():
    rospy.init_node('sm_test')

    for t in numpy.arange(-to_rad(195), to_rad(195) + 0.001, math.pi / 3):
        for b in numpy.arange(to_rad(-40), to_rad(175) + 0.001, math.pi / 3):
            for r in numpy.arange(-to_rad(145), to_rad(145) + 0.001, math.pi / 3):
                goal = MA1400JointState()
                goal.t = t
                goal.b = b
                goal.r = r
                goals.append(copy.deepcopy(goal))

    print len(goals)

    sm = smach.Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
    sm_plan_joint = gensm_plan_vis_exec(Plan1ToJointsState(), input_keys=['goal'], confirm=False)
    sm_plan_joint.userdata.robot = 2

    sm.userdata.n = -1
    with sm:
        smach.Sequence.add('counter', CounterState(len(goals)),
                           transitions={'aborted':'POWER_OFF'})
        smach.Sequence.add('pick_joints', FunctionApplicationState(pick_joints, input_keys=['n'],
                                        output_keys=['goal'], outcomes=['succeeded']))
        smach.Sequence.add('GOTO', sm_plan_joint,
                           transitions={'aborted':'counter'})
        smach.Sequence.add('PAUSE', PauseState(0.5), 
                           transitions={'succeeded':'counter'})
        smach.Sequence.add('POWER_OFF', SetServoPowerOffState())

    sis = smach_ros.IntrospectionServer('sm_test', sm, '/SM_ROOT')
    sis.start()
    os.system('clear')
    outcome = sm.execute()
    sis.stop()

if __name__ == '__main__':
    main()