prepare_simulated_robot.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2015, Fetch Robotics Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 #     * Redistributions of source code must retain the above copyright
00010 #       notice, this list of conditions and the following disclaimer.
00011 #     * Redistributions in binary form must reproduce the above copyright
00012 #       notice, this list of conditions and the following disclaimer in the
00013 #       documentation and/or other materials provided with the distribution.
00014 #     * Neither the name of the Fetch Robotics Inc. nor the names of its
00015 #       contributors may be used to endorse or promote products derived from
00016 #       this software without specific prior written permission.
00017 # 
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY
00022 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00027 # THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 
00029 # Author: Michael Ferguson
00030 
00031 import sys
00032 
00033 import rospy
00034 import actionlib
00035 from control_msgs.msg import (FollowJointTrajectoryAction,
00036                               FollowJointTrajectoryGoal,
00037                               GripperCommandAction,
00038                               GripperCommandGoal)
00039 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00040 
00041 arm_joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
00042               "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
00043 arm_intermediate_positions  = [1.32, 0, -1.4, 1.72, 0.0, 1.66, 0.0]
00044 arm_joint_positions  = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
00045 
00046 head_joint_names = ["head_pan_joint", "head_tilt_joint"]
00047 head_joint_positions = [0.0, 0.0]
00048 
00049 if __name__ == "__main__":
00050     rospy.init_node("prepare_simulated_robot")
00051 
00052     # Check robot serial number, we never want to run this on a real robot!
00053     if rospy.get_param("robot/serial") != "ABCDEFGHIJKLMNOPQRSTUVWX":
00054         rospy.logerr("This script should not be run on a real robot")
00055         sys.exit(-1)
00056 
00057     rospy.loginfo("Waiting for head_controller...")
00058     head_client = actionlib.SimpleActionClient("head_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
00059     head_client.wait_for_server()
00060     rospy.loginfo("...connected.")
00061 
00062     rospy.loginfo("Waiting for arm_controller...")
00063     arm_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
00064     arm_client.wait_for_server()
00065     rospy.loginfo("...connected.")
00066 
00067     rospy.loginfo("Waiting for gripper_controller...")
00068     gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
00069     gripper_client.wait_for_server()
00070     rospy.loginfo("...connected.")
00071 
00072     trajectory = JointTrajectory()
00073     trajectory.joint_names = head_joint_names
00074     trajectory.points.append(JointTrajectoryPoint())
00075     trajectory.points[0].positions = head_joint_positions
00076     trajectory.points[0].velocities = [0.0] * len(head_joint_positions)
00077     trajectory.points[0].accelerations = [0.0] * len(head_joint_positions)
00078     trajectory.points[0].time_from_start = rospy.Duration(5.0)
00079 
00080     head_goal = FollowJointTrajectoryGoal()
00081     head_goal.trajectory = trajectory
00082     head_goal.goal_time_tolerance = rospy.Duration(0.0)
00083 
00084     trajectory = JointTrajectory()
00085     trajectory.joint_names = arm_joint_names
00086     trajectory.points.append(JointTrajectoryPoint())
00087     trajectory.points[0].positions = [0.0] * len(arm_joint_positions)
00088     trajectory.points[0].velocities =  [0.0] * len(arm_joint_positions)
00089     trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
00090     trajectory.points[0].time_from_start = rospy.Duration(1.0)
00091     trajectory.points.append(JointTrajectoryPoint())
00092     trajectory.points[1].positions = arm_intermediate_positions
00093     trajectory.points[1].velocities =  [0.0] * len(arm_joint_positions)
00094     trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions)
00095     trajectory.points[1].time_from_start = rospy.Duration(4.0)
00096     trajectory.points.append(JointTrajectoryPoint())
00097     trajectory.points[2].positions = arm_joint_positions
00098     trajectory.points[2].velocities =  [0.0] * len(arm_joint_positions)
00099     trajectory.points[2].accelerations = [0.0] * len(arm_joint_positions)
00100     trajectory.points[2].time_from_start = rospy.Duration(7.5)
00101 
00102     arm_goal = FollowJointTrajectoryGoal()
00103     arm_goal.trajectory = trajectory
00104     arm_goal.goal_time_tolerance = rospy.Duration(0.0)
00105 
00106     gripper_goal = GripperCommandGoal()
00107     gripper_goal.command.max_effort = 10.0
00108     gripper_goal.command.position = 0.1
00109 
00110     rospy.loginfo("Setting positions...")
00111     head_client.send_goal(head_goal)
00112     arm_client.send_goal(arm_goal)
00113     gripper_client.send_goal(gripper_goal)
00114     gripper_client.wait_for_result(rospy.Duration(5.0))
00115     arm_client.wait_for_result(rospy.Duration(6.0))
00116     head_client.wait_for_result(rospy.Duration(6.0))
00117     rospy.loginfo("...done")


fetch_gazebo
Author(s): Michael Ferguson
autogenerated on Wed Apr 3 2019 02:48:45