class tambourine_text:
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
gripper = moveit_commander.MoveGroupCommander("gripper")
run = False
###
### グローバル宣言
###
move_max_velocity_value = 0.2
stop_time = 2.0 # 停止する時間を指定
force_hold_stick = 0.2 # 棒を握る力を指定
### 縦
tambourine_x_position_vertical = 0.043040 # タンバリンの手前のx座標を指定
tambourine_y_position_vertical = 0.303386 # タンバリンのy座標を指定
tambourine_z_position_vertical = 0.085469 # タンバリンの少し上のz座標を指定
stick_angle_vertical = 1.3 # 棒の角度を指定
tambourine_x_position_vertical1 = 0.043040 # タンバリンの手前のx座標を指定
tambourine_y_position_vertical1 = 0.303386 # タンバリンのy座標を指定
tambourine_z_position_vertical1 = 0.485469
"""
現在縦持ちで仮定しているためコメントアウト
### 横
tambourine_x_position_horizontal = 0.005 # タンバリンの手前のx座標を指定
tambourine_y_position_horizontal = 0.180 # タンバリンのy座標を指定
tambourine_z_position_horizontal = 0.141 # タンバリンの少し上のz座標を指定
stick_angle_horizontal = -3.14 * 9.0 / 10.0 # 棒の角度を指定
"""
###
### 変数宣言ここまで
###
def main (self):
##rospy.init_node("crane_x7_pick_and_place_controller")
self.run = True
self.robot = moveit_commander.RobotCommander()
self.arm = moveit_commander.MoveGroupCommander("arm")
self.gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(self.robot.get_group_names())
print("Current state:")
print(self.robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = self.arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
"""
現在縦持ちで仮定しているためコメントアウト
# 横持ち
# タンバリンをたたくために位置移動
def preparation_horizontal():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = tambourine_x_position_horizontal
target_pose.position.y = tambourine_y_position_horizontal
target_pose.position.z = tambourine_z_position_horizontal
q = quaternion_from_euler(-3.14, 0, 3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
# 角度を変化させタンバリンを叩く
def hit_tambourine_horizontal():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = tambourine_x_position_horizontal
target_pose.position.y = tambourine_y_position_horizontal
target_pose.position.z = tambourine_z_position_horizontal
q = quaternion_from_euler(stick_angle_horizontal, 0, 3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
"""
# SRDFに定義されている"home"の姿勢にする
self.arm.set_named_target("home")
self.arm.go()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = self.tambourine_x_position_vertical1
target_pose.position.y = self.tambourine_y_position_vertical1
target_pose.position.z = self.tambourine_z_position_vertical1
q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = self.tambourine_x_position_vertical1
target_pose.position.y = self.tambourine_y_position_vertical1
target_pose.position.z = self.tambourine_z_position_vertical1
q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
# SRDFに定義されている"home"の姿勢にする
self.arm.set_named_target("home")
self.arm.go()
self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value)
self.arm.set_named_target("home")
self.arm.go()
"""
現在縦持ちで仮定しているためコメントアウト
#パターン-----横持ち
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value)
self.arm.set_named_target("home")
self.arm.go()
"""
self.run = False
# ハンドを開く/ 閉じる
def move_gripper(self,pou):
self.gripper.set_joint_value_target([pou, pou])
self.gripper.go()
# アームを移動する
def move_arm(self,pos_x, pos_y, pos_z):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = pos_z
q = quaternion_from_euler(-3.14/2.0, 3.14, -3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
# 縦持ち
# タンバリンをたたくために位置移動
def preparation_vertical(self):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = self.tambourine_x_position_vertical
target_pose.position.y = self.tambourine_y_position_vertical
target_pose.position.z = self.tambourine_z_position_vertical
q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
# 角度を変化させタンバリンを叩く
def hit_tambourine_vertical(self):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = self.tambourine_x_position_vertical
target_pose.position.y = self.tambourine_y_position_vertical
target_pose.position.z = self.tambourine_z_position_vertical
q = quaternion_from_euler(3.14 * 9 / 10, self.stick_angle_vertical, -3.14) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
def main():
try:
### Set up the moveit_commander
print "============ Setting up the moveit_commander (press ctrl-d to exit) ..."
group = moveit_commander.MoveGroupCommander("right_hand")
### Go to the given joint state
print "============ Go to initial joint state ..."
joint_goal = np.zeros(12).tolist()
group.go(joint_goal, wait=True)
group.stop()
### Planning of two ur5 arms: go to pose goal
print "========== Load joint trajectory from h5 file "
file_name = 'glove_test_data'
group_name = 'right_glove_test_1' #'right_glove_test_1'
f = h5py.File(file_name + '.h5', "r")
timestamps = f[group_name + '/time'][:]
r_glove_angles = f[group_name + '/r_glove_angle'][:]
r_glove_angles = r_glove_angles * pi / 180 # convert degree to radius!!!
r_hand_angles = map_glove_to_inspire_hand(
r_glove_angles) # linearly map to inspire hand
### Create a plan using the imported trajectory
print "========== Create a plan from imported trajectory "
joint_plan = moveit_msgs.msg.RobotTrajectory()
joint_plan.joint_trajectory.header.frame_id = '/world'
joint_plan.joint_trajectory.joint_names = [
'Link1', 'Link11', 'Link2', 'Link22', 'Link3', 'Link33', 'Link4',
'Link44', 'Link5', 'Link51', 'Link52', 'Link53'
]
for i in range(r_hand_angles.shape[0]):
traj_point = trajectory_msgs.msg.JointTrajectoryPoint()
traj_point.positions = r_hand_angles[i, :].tolist()
t = rospy.Time(
i * 1.0 / 15.0
) #rospy.Time(timestamps[i]) # to coordinate with the 15 Hz frame rate of the exported video (though it's not the original timestamps)
traj_point.time_from_start.secs = t.secs
traj_point.time_from_start.nsecs = t.nsecs
joint_plan.joint_trajectory.points.append(
copy.deepcopy(traj_point))
### Go to the start point
print "========== Go to the starting position "
joint_goal = joint_plan.joint_trajectory.points[0].positions
group.go(joint_goal, wait=True)
group.stop()
### Execute a plan
print "============ Execute a saved path ..."
import pdb
pdb.set_trace()
group.execute(joint_plan, wait=True)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
#! /usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
rospy.init_node('mover4_move_group_python_interface', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("robot")
# gripper = moveit_commander.MoveGroupCommander("gripper")
rospy.loginfo("release_bottle_on_tall_table execute.py")
group.get_current_pose()
group.set_named_target("release_bottle_on_tall_table")
group.go(wait=True)
moveit_commander.roscpp_shutdown()
if __name__ == '__main__':
# intialize moveit commander and rospy node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pick_and_place', anonymous = True)
rospy.wait_for_service("/clear_octomap")
clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
gripper = actionlib.SimpleActionClient("iiwa/gripper_controller/gripper_cmd", GripperCommandAction)
gripper.wait_for_server()
manipulator_name = "manipulator" #give groupname you want to shoe
endeffector_name = "gripper"
robot = moveit_commander.RobotCommander()
manipulator = moveit_commander.MoveGroupCommander(manipulator_name)
endeffector = moveit_commander.MoveGroupCommander(endeffector_name)
# manipulator.allow_replanning(True)
tf_listener = tf.TransformListener()
rate = rospy.Rate(10)
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 10.0
#this object is the interface to the world surrounding the robot
scene = moveit_commander.PlanningSceneInterface()
### Create Environment
create_environment() #Create the environment
#Is used to display the trajectoryies in RVIZ
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size = 20)
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This object is an interface
## to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to one group of joints. In this case the group is the joints in the Panda
## arm so we set ``group_name = panda_arm``. If you are using a different robot,
## you should change this value to the name of your robot arm planning group.
## This interface can be used to plan and execute motions on the Panda:
group_name = "right_hand"
group = moveit_commander.MoveGroupCommander(group_name)
## We create a `DisplayTrajectory`_ publisher which is used later to publish
## trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## END_SUB_TUTORIAL
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
# Moving to a pose goal
group.execute(plan1, wait=True)
rospy.sleep(0.5)
if __name__ == "__main__":
rospy.init_node("move_cubes")
models = rospy.ServiceProxy("/gazebo/get_world_properties", GetWorldProperties)
model_coordinates = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
# initializing moveit related things
moveit_commander.roscpp_initialize(sys.argv)
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander("Arm")
display_trajectory_publisher = rospy.Publisher(
"/move_group/display_planned_path",
moveit_msgs.msg.DisplayTrajectory,
queue_size=1e3,
)
rospy.sleep(2)
group.set_goal_orientation_tolerance(0.01)
group.set_goal_tolerance(0.01)
group.set_goal_joint_tolerance(0.01)
group.set_planning_time(1e3)
p = geometry_msgs.msg.PoseStamped()
import tf
import sys
import rospy
import moveit_msgs.msg
import moveit_commander
import geometry_msgs.msg
if __name__ == '__main__':
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_to_tf', )
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander('manipulator')
# Override default planner
group.set_planner_id('RRTConnectkConfigDefault')
target_tf = rospy.get_param('~target_tf', '/target')
base_tf = rospy.get_param('~base_tf', '/base_link')
test_mode = rospy.get_param('~test_mode', False)
x = rospy.get_param('~x', 0.1)
y = rospy.get_param('~y', 0.4)
z = rospy.get_param('~z', 0.8)
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rate = rospy.Rate(10.0)
from math import pi
from tf.transformations import *
if __name__ == "__main__":
# Initialize moveit_commander and node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('d3_move_target_pose', anonymous=False)
# Get instance from moveit_commander
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# Get group_commander from MoveGroupCommander
group_name = "indy7"
move_group = moveit_commander.MoveGroupCommander(group_name)
# Move using target_pose
pose_goal = geometry_msgs.msg.Pose()
pose_goal.position.x = 0.0
pose_goal.position.y = -0.6
pose_goal.position.z = 0.8
pose_goal.orientation.x = -0.489606100026
pose_goal.orientation.y = -0.500430269326
pose_goal.orientation.z = 0.49569733542
pose_goal.orientation.w = 0.513945098252
def _init_planning_interface(self):
""" Initializes the MoveIn planning interface """
self._robot_comm = moveit_commander.RobotCommander()
self._planning_group = moveit_commander.MoveGroupCommander(self._arm)
self._planning_group.set_pose_reference_frame(ymc.MOVEIT_PLANNING_REFERENCE_FRAME)
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface', anonymous=True)
## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This object is an interface
## to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
ur5_manipulator = moveit_commander.MoveGroupCommander("ur5_manipulator")
ur5_gripper = moveit_commander.MoveGroupCommander("ur5_gripper")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=1)
ur5_manipulator.set_pose_reference_frame('base_link')
end_effector_link = ur5_manipulator.get_end_effector_link()
#[-0.7068246908767175, 4.987609811687592e-07, -0.7073887590049329, 4.946056750221776e-07]
pose_taget = geometry_msgs.msg.Pose()
pose_taget.position.x = 0.4
pose_taget.position.y = 0.2
pose_taget.position.z = 0.180
pose_taget.orientation.x = 0.415
pose_taget.orientation.y = -0.436
pose_taget.orientation.z = 0.772
def __init__(self):
self.track_flag = False
self.default_pose_flag = True
self.area_flag = False
self.cx = 400.0
self.cy = 400.0
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('/ur5/usbcam/image_raw', Image, self.image_callback)
rospy.init_node("moveit_cartesian_path", anonymous=False)
self.state_change_time = rospy.Time.now()
rospy.loginfo("Starting node moveit_cartesian_path")
rospy.on_shutdown(self.cleanup)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander('manipulator')
# Get the name of the end-effector link
self.end_effector_link = self.arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = "/base_link"
# Set the ur5_arm reference frame accordingly
self.arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
# Get the current pose so we can add it as a waypoint
start_pose = self.arm.get_current_pose(self.end_effector_link).pose
# Initialize the waypoints list
self.waypoints= []
# Set the first waypoint to be the starting pose
# Append the pose to the waypoints list
wpose = deepcopy(start_pose)
# Set the next waypoint to the right 0.5 meters
wpose.position.x = -0.2
wpose.position.y = -0.2
wpose.position.z = 0.3
self.waypoints.append(deepcopy(wpose))
wpose.position.x = 0.1052
wpose.position.y = -0.4271
wpose.position.z = 0.4005
wpose.orientation.x = 0.4811
wpose.orientation.y = 0.4994
wpose.orientation.z = -0.5121
wpose.orientation.w = 0.5069
self.waypoints.append(deepcopy(wpose))
if np.sqrt((wpose.position.x-start_pose.position.x)**2+(wpose.position.x-start_pose.position.x)**2 \
+(wpose.position.x-start_pose.position.x)**2)<0.1:
rospy.loginfo("Warnig: target position overlaps with the initial position!")
# self.arm.set_pose_target(wpose)
# Set the internal state to the current state
self.arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
"""moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path(
self, waypoints, eef_step, jump_threshold, avoid_collisios= True)
Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the
poses specified as waypoints. Configurations are computed for every eef_step meters;
The jump_threshold specifies the maximum distance in configuration space between consecutive points
in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed,
the actual RobotTrajectory.
"""
plan, fraction = self.arm.compute_cartesian_path(self.waypoints, 0.01, 0.0, True)
# plan = self.arm.plan()
# If we have a complete plan, execute the trajectory
if 1-fraction < 0.2:
rospy.loginfo("Path computed successfully. Moving the arm.")
num_pts = len(plan.joint_trajectory.points)
rospy.loginfo("\n# intermediate waypoints = "+str(num_pts))
self.arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed")
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_ik_demo')
# 初始化需要使用move group控制的机械臂中的arm group
rarm = moveit_commander.MoveGroupCommander('R_xarm')
rgripper = moveit_commander.MoveGroupCommander('R_gripper')
# 获取终端link的名称
rend_effector_link = rarm.get_end_effector_link()
# 设置目标位置所使用的参考坐标系
reference_frame = 'bottom_link'
rarm.set_pose_reference_frame(reference_frame)
# 当运动规划失败后，允许重新规划
rarm.allow_replanning(True)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
rarm.set_goal_position_tolerance(0.001)
rarm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
rarm.set_max_acceleration_scaling_factor(0.5)
rarm.set_max_velocity_scaling_factor(0.5)
# 控制机械臂先回到初始化位置
#arm.set_named_target('home')
#arm.go()
#rospy.sleep(1)
# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
# 姿态使用四元数描述，基于base_link坐标系
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = -0.0687
target_pose.pose.position.y = 0
target_pose.pose.position.z = 0.77
target_pose.pose.orientation.x = 1.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 0.0
# 设置机器臂当前的状态作为运动初始状态
rarm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
rarm.set_pose_target(target_pose, rend_effector_link)
# 规划运动路径
traj = rarm.plan()
# 按照规划的运动路径控制机械臂运动
rarm.execute(traj)
rospy.sleep(1)
rgripper.set_joint_value_target([0.67])
rgripper.go()
rospy.sleep(1)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = -0.0776
target_pose.pose.position.y = 0
target_pose.pose.position.z = 0.85
target_pose.pose.orientation.x = 1.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 0.0
# 设置机器臂当前的状态作为运动初始状态
rarm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
rarm.set_pose_target(target_pose, rend_effector_link)
# 规划运动路径
traj = rarm.plan()
# 按照规划的运动路径控制机械臂运动
rarm.execute(traj)
rospy.sleep(1)
rgripper.set_joint_value_target([0.1])
rgripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def main():
moveit_commander.roscpp_initialize(sys.argv)
manipulator_group = moveit_commander.MoveGroupCommander("manipulator")
manipulator_group.set_pose_reference_frame('base_link')
# Set the allowable error of position (unit: meter) and attitude (unit: radians)
manipulator_group.set_goal_position_tolerance(0.05)
manipulator_group.set_goal_orientation_tolerance(0.05)
# Get the name of the terminal link
end_effector_link = manipulator_group.get_end_effector_link()
group_variable_values = manipulator_group.get_current_joint_values()
group_variable_values[0] = 2.6878
group_variable_values[1] = -2.1817
group_variable_values[2] = -0.2094
group_variable_values[3] = -2.4958
group_variable_values[4] = 1.5533
group_variable_values[5] = 1.4312
manipulator_group.set_joint_value_target(group_variable_values)
plan = manipulator_group.plan()
manipulator_group.go(wait=True)
init_pose = []
init_pose.append(0.278694)
init_pose.append(-0.262539)
init_pose.append(0.401485)
# Get the current pose data as the starting pose of the robot arm movement
start_pose = manipulator_group.get_current_pose(end_effector_link).pose
wpose = deepcopy(start_pose)
waypoints=[]
wpose.position.x += float(rows[12][0]) - 0.02289 - init_pose[0] - 0.0233
wpose.position.y += float(rows[12][1]) + 0.011618 - init_pose[1] - 0.045
wpose.position.z += float(rows[12][2]) + 0.165994 - init_pose[2] + 0.01
waypoints.append(deepcopy(wpose))
(plan, fraction) = manipulator_group.compute_cartesian_path (waypoints, 0.01,0.0,True)
manipulator_group.execute(plan)
# waypoints=[]
# wpose.orientation.w = -0.331305
# wpose.orientation.x = -0.498117
# wpose.orientation.y = -0.493528
# wpose.orientation.z = 0.631306
# waypoints.append(deepcopy(wpose))
# (plan, fraction) = manipulator_group.compute_cartesian_path (waypoints, 0.01,0.0,True)
# manipulator_group.execute(plan)
joint_target = manipulator_group.get_current_joint_values()
joint_target[0] = 2.68781
joint_target[1] = -2.0245
joint_target[2] = -0.6806
joint_target[3] = -2.28638
joint_target[4] = 1.518
joint_target[5] = 1.4311
manipulator_group.set_joint_value_target(joint_target)
plan = manipulator_group.plan()
manipulator_group.go(wait=True)
waypoints=[]
wpose.position.x += 0.02256
wpose.position.y -= 0.0086
wpose.position.z += 0.0131
waypoints.append(deepcopy(wpose))
(plan, fraction) = manipulator_group.compute_cartesian_path (waypoints, 0.01,0.0,True)
manipulator_group.execute(plan)
#-----SCAN-----
sub = rospy.wait_for_message("fiducial_transforms", FiducialTransformArray)
tag_id = save_id(sub)
print(tag_id)
waypoints=[]
wpose.position.x -= 0.0088
wpose.position.y += 0.0023
wpose.position.z -= 0.0296
waypoints.append(deepcopy(wpose))
group_variable_values = manipulator_group.get_current_joint_values()
group_variable_values[0] = 2.6878
group_variable_values[1] = -2.14675
group_variable_values[2] = -0.5235
group_variable_values[3] = -2.426
group_variable_values[4] = 1.5009
group_variable_values[5] = 1.44862
manipulator_group.set_joint_value_target(group_variable_values)
plan = manipulator_group.plan()
manipulator_group.go(wait=True)
# #------SCAN------
sub = rospy.wait_for_message("fiducial_transforms", FiducialTransformArray)
tag_id = save_id(sub)
print(tag_id)
#home
joint_target = manipulator_group.get_current_joint_values()
joint_target[0] = 0
joint_target[1] = -2.0944
joint_target[2] = 1.74533
joint_target[3] = 0.34906
joint_target[4] = 1.5708
joint_target[5] = -1.5708
manipulator_group.set_joint_value_target(joint_target)
plan = manipulator_group.plan()
manipulator_group.go(wait=True)
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
def main (self):
##rospy.init_node("crane_x7_pick_and_place_controller")
self.run = True
self.robot = moveit_commander.RobotCommander()
self.arm = moveit_commander.MoveGroupCommander("arm")
self.gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(self.robot.get_group_names())
print("Current state:")
print(self.robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = self.arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
"""
現在縦持ちで仮定しているためコメントアウト
# 横持ち
# タンバリンをたたくために位置移動
def preparation_horizontal():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = tambourine_x_position_horizontal
target_pose.position.y = tambourine_y_position_horizontal
target_pose.position.z = tambourine_z_position_horizontal
q = quaternion_from_euler(-3.14, 0, 3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
# 角度を変化させタンバリンを叩く
def hit_tambourine_horizontal():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = tambourine_x_position_horizontal
target_pose.position.y = tambourine_y_position_horizontal
target_pose.position.z = tambourine_z_position_horizontal
q = quaternion_from_euler(stick_angle_horizontal, 0, 3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
"""
# SRDFに定義されている"home"の姿勢にする
self.arm.set_named_target("home")
self.arm.go()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = self.tambourine_x_position_vertical1
target_pose.position.y = self.tambourine_y_position_vertical1
target_pose.position.z = self.tambourine_z_position_vertical1
q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = self.tambourine_x_position_vertical1
target_pose.position.y = self.tambourine_y_position_vertical1
target_pose.position.z = self.tambourine_z_position_vertical1
q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
# SRDFに定義されている"home"の姿勢にする
self.arm.set_named_target("home")
self.arm.go()
self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value)
self.arm.set_named_target("home")
self.arm.go()
"""
現在縦持ちで仮定しているためコメントアウト
#パターン-----横持ち
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value)
self.arm.set_named_target("home")
self.arm.go()
"""
self.run = False
def main():
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(1.0)
gripper = moveit_commander.MoveGroupCommander("gripper")
PRESET_DEFAULT = 0
PRESET_FREE = 3
pid_gain_no = PRESET_DEFAULT
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
# 現在のアーム姿勢を、目標姿勢にセットする
joy_wrapper.set_target_arm(arm.get_current_pose())
joy_wrapper.set_target_gripper(gripper.get_current_joint_values())
while joy_wrapper.do_shutdown() == False:
# グリッパーの角度を変更する
if joy_wrapper.get_and_reset_grip_update_flag():
gripper.set_joint_value_target(joy_wrapper.get_target_gripper())
gripper.go()
# アームの姿勢を変更する
if joy_wrapper.get_and_reset_pose_update_flag():
arm.set_pose_target(joy_wrapper.get_target_arm())
if arm.go() == False:
# 現在のアーム姿勢を、目標姿勢にセットする
joy_wrapper.set_target_arm(arm.get_current_pose())
# アームの姿勢をpose_group (home or vertical)に変更する
if joy_wrapper.get_and_reset_name_update_flag():
arm.set_named_target(joy_wrapper.get_target_name())
arm.go()
# 現在のアーム姿勢を、目標姿勢にセットする
joy_wrapper.set_target_arm(arm.get_current_pose())
# アームのPIDゲインをプリセットする
if joy_wrapper.get_and_reset_preset_update_flag():
# PIDゲインを0:default, 1:Free に切り替える
if pid_gain_no == PRESET_DEFAULT:
pid_gain_no = PRESET_FREE
else:
pid_gain_no = PRESET_DEFAULT
# PIDゲインをdefaultに戻すと、目標姿勢に向かって急に動き出す
# 安全のため、現在のアームの姿勢を目標姿勢に変更する
arm.set_pose_target(arm.get_current_pose())
arm.go()
preset_pid_gain(pid_gain_no)
# 現在のアーム姿勢を、目標姿勢にセットする
joy_wrapper.set_target_arm(arm.get_current_pose())
# ティーチング
teaching_flag = joy_wrapper.get_and_reset_teaching_flag()
if teaching_flag == joy_wrapper.TEACHING_SAVE:
# 現在のアーム姿勢、グリッパー角度を保存する
# アームの角度が制御範囲内にない場合、例外が発生する
try:
arm_joint_values = arm.get_current_joint_values()
gripper_joint_values = gripper.get_current_joint_values()
arm.set_joint_value_target(arm_joint_values)
gripper.set_joint_value_target(gripper_joint_values)
joy_wrapper.save_joint_values(arm_joint_values, gripper_joint_values)
except moveit_commander.exception.MoveItCommanderException:
print("Error setting joint target. Is the target within bounds?")
elif teaching_flag == joy_wrapper.TEACHING_LOAD:
# 保存したアーム、グリッパー角度を取り出す
joint_values = joy_wrapper.load_joint_values()
if joint_values:
arm.set_joint_value_target(joint_values[0])
arm.go()
gripper.set_joint_value_target(joint_values[1])
gripper.go()
# 現在のアーム姿勢を、目標姿勢にセットする
joy_wrapper.set_target_arm(arm.get_current_pose())
joy_wrapper.set_target_gripper(gripper.get_current_joint_values())
elif teaching_flag == joy_wrapper.TEACHING_DELETE:
# 保存したアーム姿勢、グリッパー角度を削除する
joy_wrapper.delete_joint_values()
rospy.loginfo("Shutdown...")
# SRDFに定義されている"vertical"の姿勢にする
arm.set_named_target("vertical")
arm.go()
def main():
rospy.init_node("crane_x7_pick_and_place_controller")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.1)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
#print("Group names:")
#print(robot.get_group_names())
#print("Current state:")
# print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
#print("Arm initial pose:")
#print(arm_initial_pose)
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴む準備をする
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0
target_pose.position.y = -0.3
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴みに行く
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0
target_pose.position.y = -0.3
target_pose.position.z = 0.07
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを閉じる
gripper.set_joint_value_target([0.2, 0.2])
gripper.go()
# SRDFに定義されている"landing"の姿勢にする
arm.set_named_target("landing")
arm.go()
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# SRDFに定義されている"vertical"の姿勢にする
#アルコールに当たることがあるので一度アームを垂直にする。
arm.set_named_target("vertical")
arm.go()
#アームを開いて押す準備をする
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
#アームをアルコールの上に持ってくる
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.1
target_pose.position.z = 0.33
q = quaternion_from_euler(-3.14, 0.0,
-0.7) #手首をz軸に80度ほど回転させた姿勢にしてボトルのノズルを押しやすくする。
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
#アルコールボトルを押す
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.1
target_pose.position.z = 0.21
q = quaternion_from_euler(-3.14, 0, -0.7) #先程の手首をz軸に80度ほど回転させた姿勢にする。
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
rospy.sleep(1.0)
#アルコールボトルからアームを引く
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.1
target_pose.position.z = 0.33
q = quaternion_from_euler(-3.14, 0.0, -0.7) #先程の手首をz軸に80度ほど回転させた姿勢にする。
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
#home(元々の)の姿勢に戻す。
arm.set_named_target("home")
arm.go()
print("done")
# initialize moveit_commander and rospy
print "============ Starting movement setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('bin_picking_move_fanuc', anonymous=True)
# This RobotCommander object is an interface to the robot as a whole.
robot = moveit_commander.RobotCommander()
# This PlanningSceneInterface object is an interface to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
# MoveGroupCommander object. This object is an interface to one group of joints.
# In this case the group is the joints in the manipulator. This interface can be used
# to plan and execute motions on the manipulator.
group = moveit_commander.MoveGroupCommander("manipulator")
rate = rospy.Rate(10) # 10hz
# Publisher of pointStamped of the grasping point
grasping_point_pub = rospy.Publisher('/graspingPoint',
PointStamped,
queue_size=10)
# Publisher of pointStamped of the grasping point
io_pub = rospy.Publisher('/io_client_messages', ios, queue_size=10)
normal = Vector3()
approx_point = Vector3()
eef_position_laser = Vector3()
roll = np.pi
pitch = Float32()
import moveit_msgs.msg
import geometry_msgs.msg
import tf_conversions
import geometry_msgs.msg
from tf import TransformListener
print("============ Starting tutorial setup")
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
tf_listener = TransformListener()
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
arm = moveit_commander.MoveGroupCommander("arm")
gripper = moveit_commander.MoveGroupCommander("gripper")
rospy.sleep(1)
arm.set_goal_tolerance(0.003)
arm.set_max_velocity_scaling_factor(1.0)
print("current pose: ", arm.get_current_pose())
print("current rpy: ", arm.get_current_rpy())
print("joints: ", arm.get_current_joint_values())
print("============ Generating plan 1")
pose_target = geometry_msgs.msg.PoseStamped()
def __init__(self):
rospy.init_node("moveit_cartesian_path", anonymous=False)
rospy.loginfo("Starting node moveit_cartesian_path")
rospy.on_shutdown(self.cleanup)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander('manipulator')
# Get the name of the end-effector link
end_effector_link = self.arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = "/base_link"
# Set the ur5_arm reference frame accordingly
self.arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
# Get the current pose so we can add it as a waypoint
start_pose = self.arm.get_current_pose(end_effector_link).pose
#print "start pose", start_pose
orientation_list = [
start_pose.orientation.x, start_pose.orientation.y,
start_pose.orientation.z, start_pose.orientation.w
]
#print orientation_list
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
print roll, pitch, yaw
#roll += 0.03
quat = quaternion_from_euler(roll, pitch, yaw)
#print quat
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
wpose.position.x = 0.724878225068
wpose.position.y = -0.0902229262006
wpose.position.z = -0.0132514994323
wpose.orientation.x = -0.425978280494
wpose.orientation.y = 0.553721152459
wpose.orientation.z = 0.44131806175
wpose.orientation.w = 0.563181816326
waypoints.append(deepcopy(wpose))
fraction = 0.0
maxtries = 100
attempts = 0
#print waypoints
# Set the internal state to the current state
self.arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.01, 0.0, True)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
import numpy as np
import moveit_commander
import geometry_msgs.msg
from pr2_controllers_msgs.msg import Pr2GripperCommand
sys.path.append(os.path.join(os.path.dirname(__file__), '..'))
from constants import TEST_GRIPPER_POS as GRIPPER_POS
TOPIC_NAME = '/r_gripper_controller/command'
pub_gripper = rospy.Publisher(TOPIC_NAME, Pr2GripperCommand, queue_size=10)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pr2_move_right_arm_test')
robot = moveit_commander.RobotCommander()
right_arm = moveit_commander.MoveGroupCommander('right_arm')
right_gripper = moveit_commander.MoveGroupCommander('right_gripper')
group_names = robot.get_group_names()
print('Available Planning Groups:', robot.get_group_names())
print('Current state:', robot.get_current_state())
def right_pose_pos(x, y, z):
pose_goal = geometry_msgs.msg.Pose()
pose_goal.position.x = x
pose_goal.position.y = y
pose_goal.position.z = z
pose_goal.orientation.x = GRIPPER_POS[0]
pose_goal.orientation.y = GRIPPER_POS[1]
pose_goal.orientation.z = GRIPPER_POS[2]
def move_pick_cube():
print("============ Starting setup ============")
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_pick_cube', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("Arm")
end_effector_link = group.get_end_effector_link()
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
print("============ Reference frame: %s" % group.get_planning_frame())
print("============ End effector frame: %s" %
group.get_end_effector_link())
print("============ Robot Groups:")
print(robot.get_group_names())
print("============ Printing robot state")
print(robot.get_current_state())
print("============")
print("============put cubes and bucket into moveit workspace")
# If we're coming from another script we might want to remove the objects
for i in xrange(0, 6):
if "cube{}".format(i) in scene.get_known_object_names():
scene.remove_world_object("cube{}".format(i))
if "bucket" in scene.get_known_object_names():
scene.remove_world_object("bucket")
##put cubes and bucket into moveit workspace
##load param into a list
cube_bucket_orient = rospy.get_param("cube_bucket_orientRPY")
bucket_pose = rospy.get_param("bucket_XYZ")
cube_num = rospy.get_param("cube_num")
table_height = 0.75 #table height=0.8
orient_quat = geometry_msgs.msg.Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
cube_bucket_orient[0], cube_bucket_orient[1],
cube_bucket_orient[2]))
cube_pose_list = list()
for i in xrange(0, cube_num):
if rospy.has_param("cube{}_XYZ".format(i)):
cube_pose_list.append(rospy.get_param("cube{}_XYZ".format(i)))
cube_pose_list[i][2] = table_height
##prepare the cubes info
cube_info = geometry_msgs.msg.PoseStamped()
cube_info.header.frame_id = "robot_base"
for i in xrange(0, cube_num):
cube_info.pose.position.x = cube_pose_list[i][0]
cube_info.pose.position.y = cube_pose_list[i][1]
cube_info.pose.position.z = table_height #table height
cube_info.pose.orientation = orient_quat
print("cube{} x:{}, y:{}".format(i, cube_pose_list[i][0],
cube_pose_list[i][1]))
scene.add_box("cube{}".format(i), cube_info,
(0.05, 0.05, 0.05)) ##found in cubic.urdf
#prepare the bucket info
bucket_info = geometry_msgs.msg.PoseStamped()
bucket_info.header.frame_id = "robot_base"
bucket_info.pose.position.x = bucket_pose[0]
bucket_info.pose.position.y = bucket_pose[1]
bucket_info.pose.position.z = table_height
bucket_info.pose.orientation = orient_quat
scene.add_box("bucket", bucket_info,
(0.2, 0.2, 0.2)) ##cannot found in bucket.dae
print("============moveit workspace loading finished")
## setup the planner parameters
group.set_goal_orientation_tolerance(0.01)
group.set_goal_tolerance(0.01)
group.set_goal_joint_tolerance(0.01)
group.set_num_planning_attempts(100)
pub = rospy.Publisher("/jaco/joint_control", JointState, queue_size=1)
#################make hand towards table
print("hand towards table")
hand_pick_quat = geometry_msgs.msg.Quaternion(0.347709304721,
-0.646715871525,
0.298597553716,
0.609669026475)
pose_goal = group.get_current_pose(end_effector_link).pose
ini_pose = pose_goal
pose_goal.orientation = hand_pick_quat
group.set_start_state_to_current_state()
group.set_pose_target(pose_goal, end_effector_link)
plan1_1 = group.plan()
group.execute(plan1_1)
base_x = 0.5788
base_y = -0.0154
dis_threshold_far = 0.6
dis_threshold_near = 0.26
for i_cube_check in xrange(0, cube_num):
dis = np.sqrt(
np.square(cube_pose_list[i_cube_check][0] - base_x) +
np.square(cube_pose_list[i_cube_check][1] - base_y))
if dis > dis_threshold_far:
print(
'cube{} is at a dangerous position(too far) in a distance:{} to the robot base, the task of picking cube{} might fail'
.format(i_cube_check, dis, i_cube_check))
elif dis < dis_threshold_near:
print(
'cube{} is at a dangerous position(too near) in a distance:{} to the robot base, the task of picking cube{} might fail'
.format(i_cube_check, dis, i_cube_check))
else:
print(
'cube{} is at a reachable position in a distance:{} to the robot base'
.format(i_cube_check, dis))
for i_cube in xrange(0, cube_num):
######################### middle point
print("cube{} go to middle point".format(i_cube))
pose_goal.position.x = 0.30
pose_goal.position.y = 0.0
group.set_start_state_to_current_state()
group.set_pose_target(pose_goal, end_effector_link)
plan1_2 = group.plan()
group.execute(plan1_2)
rospy.sleep(1.)
########################### go to the upper of cube
print("cube{} go to the upper of cube".format(i_cube))
waypoints12 = list()
pose_goal12 = group.get_current_pose(end_effector_link).pose
ini_pose12 = pose_goal12
waypoints12.append(copy.deepcopy(pose_goal12))
pose_goal12.position.x = cube_pose_list[i_cube][0]
pose_goal12.position.y = cube_pose_list[i_cube][1]
pose_goal12.position.z = table_height + 0.2
waypoints12.append(copy.deepcopy(pose_goal12))
# plan and execute
fraction = 0.0
attempts_max = 100
attempts = 0
while fraction < 0.6 and attempts < attempts_max:
(plan12,
fraction) = group.compute_cartesian_path(waypoints12, 0.01, 0.0)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
## Moving to a pose goal
if fraction > 0.4: #fraction == 1.0:
rospy.loginfo("Path planning " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
rospy.loginfo("Path computed successfully. Moving the arm.")
group.execute(plan12)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
group.set_start_state_to_current_state()
group.set_pose_target(pose_goal12, end_effector_link)
plan12 = group.plan()
group.execute(plan12)
rospy.sleep(1.)
########################go down to the cube
print("cube{} go down to the cube".format(i_cube))
waypoints2 = list()
pose_goal2 = group.get_current_pose(end_effector_link).pose
ini_pose2 = pose_goal2
waypoints2.append(copy.deepcopy(pose_goal2))
pose_goal2.position.x = cube_pose_list[i_cube][0]
pose_goal2.position.y = cube_pose_list[i_cube][1]
pose_goal2.position.z = table_height + 0.17
waypoints2.append(copy.deepcopy(pose_goal2))
fraction = 0.0
attempts_max = 100
attempts = 0
while fraction < 0.8 and attempts < attempts_max:
(plan2,
fraction) = group.compute_cartesian_path(waypoints2, 0.01, 0.0)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
## Moving to a pose goal
if fraction > 0.8: #fraction == 1.0:
rospy.loginfo("Path planning " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
rospy.loginfo("Path computed successfully. Moving the arm.")
group.execute(plan2)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
continue #the action of going down to the cube needs to be done perfectly, if not, quit
rospy.sleep(1.)
##############################################################
################################################close the hand
print("cube{} close hand".format(i_cube))
currentJointState = rospy.wait_for_message("/joint_states", JointState)
print('Received!')
currentJointState.header.stamp = rospy.get_rostime()
tmp = 0.7
# tmp_tuple=tuple([tmp] + list(currentJointState.position[1:]))
currentJointState.position = tuple(
list(currentJointState.position[:6]) + [tmp] + [tmp] + [tmp])
rate = rospy.Rate(10) # 10hz
for i in range(3):
pub.publish(currentJointState)
print('Published!')
rate.sleep()
print('end!')
rospy.sleep(5.)
#############################################
#############################################hand up
print("cube{} hand up from cube".format(i_cube))
waypoints3 = list()
pose_goal3 = group.get_current_pose(end_effector_link).pose
ini_pose3 = copy.deepcopy(pose_goal3)
#print(pose_goal3)
waypoints3.append(copy.deepcopy(pose_goal3))
pose_goal3.position.z = ini_pose3.position.z + 0.2 #0.4
waypoints3.append(copy.deepcopy(pose_goal3))
fraction = 0.0
attempts_max = 100
attempts = 0
while fraction < 1.0 and attempts < attempts_max:
(plan3,
fraction) = group.compute_cartesian_path(waypoints3, 0.01, 0.0)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
## Moving to a pose goal
if fraction > 0.4: #fraction == 1.0:
rospy.loginfo("Path planning " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
rospy.loginfo("Path computed successfully. Moving the arm.")
group.execute(plan3)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
pose_goal3.position.z = ini_pose3.position.z + 0.2 # 0.3#have to use the x,y of current pose instead of cube's pose when going up
group.set_start_state_to_current_state()
group.set_pose_target(pose_goal3, end_effector_link)
plan3 = group.plan()
group.execute(plan3)
rospy.sleep(1.)
rospy.sleep(1.)
####################################################################
###########################################go to the upper of bucket
print("cube{} go to the upper of bucket".format(i_cube))
waypoints34 = list()
pose_goal34 = group.get_current_pose(end_effector_link).pose
ini_pose34 = pose_goal34
waypoints34.append(copy.deepcopy(pose_goal34))
pose_goal34.position.x = bucket_pose[0]
pose_goal34.position.y = bucket_pose[1]
pose_goal34.position.z = table_height + 0.6
pose_goal34.orientation = hand_pick_quat
waypoints34.append(copy.deepcopy(pose_goal34))
(plan34,
fraction) = group.compute_cartesian_path(waypoints34, 0.01, 0.0)
## Moving to a pose goal
group.execute(plan34, wait=True)
rospy.sleep(1.)
############################################go to bucket
waypoints4 = list()
pose_goal4 = group.get_current_pose(end_effector_link).pose
ini_pose4 = pose_goal4
waypoints4.append(copy.deepcopy(pose_goal4))
pose_goal4.position.x = bucket_pose[0]
pose_goal4.position.y = bucket_pose[1]
pose_goal4.position.z = table_height + 0.5
pose_goal4.orientation = hand_pick_quat
#print(pose_goal4)
waypoints4.append(copy.deepcopy(pose_goal4))
(plan4, fraction) = group.compute_cartesian_path(waypoints4, 0.01, 0.0)
## Moving to a pose goal
group.execute(plan4, wait=True)
rospy.sleep(1.)
#################################open the hand
print("cube{} open hand".format(cube_num))
currentJointState = rospy.wait_for_message("/joint_states", JointState)
print('Received!')
currentJointState.header.stamp = rospy.get_rostime()
tmp = 0.005
# tmp_tuple=tuple([tmp] + list(currentJointState.position[1:]))
currentJointState.position = tuple(
list(currentJointState.position[:6]) + [tmp] + [tmp] + [tmp])
rate = rospy.Rate(10) # 10hz
for i in range(3):
pub.publish(currentJointState)
print('Published!')
rate.sleep()
print('end!')
rospy.sleep(2.)
#############################################
#############################################hand up
print("cube{} hand up from bucket".format(i_cube))
waypoints5 = list()
pose_goal5 = group.get_current_pose(end_effector_link).pose
ini_pose5 = pose_goal5
waypoints5.append(pose_goal5)
pose_goal5.position.z = table_height + 0.5 # have to use the x,y of current pose instead of bucket's pose when going up
pose_goal5.orientation = hand_pick_quat
waypoints5.append(pose_goal5)
pose_goal5.position.z = table_height + 0.6
waypoints5.append(pose_goal5)
(plan5, fraction) = group.compute_cartesian_path(waypoints5, 0.01, 0.0)
## Moving to a pose goal
group.execute(plan5, wait=True)
rospy.sleep(1.)
print("go back")
waypoints6 = list()
pose_goal6 = group.get_current_pose(end_effector_link).pose
waypoints6.append(pose_goal6)
pose_goal6.position = ini_pose.position
waypoints6.append(pose_goal6)
(plan6, fraction) = group.compute_cartesian_path(waypoints6, 0.01, 0.0)
## Moving to a pose goal
group.execute(plan6, wait=True)
rospy.sleep(1.)
# END_TUTORIAL
print("============ STOPPING")
R = rospy.Rate(10)
while not rospy.is_shutdown():
R.sleep()
import baxter_interface
import math
import tf
print "============ Starting tutorial setup"
# Se debe redirigir topic de joint_states para baxter
sys.argv.append('joint_states:=/robot/joint_states')
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('movimiento_prueba_1',anonymous=True)
robot = moveit_commander.RobotCommander()
#scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("left_arm")
# Publicador de trayectorias para que rviz las visualize.
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory, queue_size = 10)
# PLANEANDO UNA POSE GOAL
rot = tf.transformations.quaternion_from_euler(math.pi, 0, -math.pi/2)
print rot
print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
def default_pose_execute(self):
self.waypoints= []
self.pointx = []
self.pointy = []
self.phase = 1
self.object_cnt = 0
self.track_flag = False
self.default_pose_flag = True
self.cx = 320.0
self.cy = 240.0
self.points=[]
self.state_change_time = rospy.Time.now()
rospy.loginfo("Starting node moveit_cartesian_path")
rospy.on_shutdown(self.cleanup)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander('manipulator')
# Get the name of the end-effector link
self.end_effector_link = self.arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = "/base_link"
# Set the ur5_arm reference frame accordingly
self.arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
self.arm.set_planning_time(0.1)
self.arm.set_max_acceleration_scaling_factor(.4)
self.arm.set_max_velocity_scaling_factor(.65)
self.shoulder_lift_vel_name = '/robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity'
self.shoulder_pan_vel_name = '/robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity'
self.elbow_vel_name = '/robot_description_planning/joint_limits/elbow_joint/max_velocity'
# Get the current pose so we can add it as a waypoint
start_pose = self.arm.get_current_pose(self.end_effector_link).pose
# Initialize the waypoints list
self.waypoints= []
self.pointx = []
self.pointy = []
# Set the first waypoint to be the starting pose
# Append the pose to the waypoints list
# Default pose in Cartesian frame
wpose = deepcopy(start_pose)
# Set the next waypoint to the right 0.5 meters
wpose.position.x = -0.1123
wpose.position.y = -0.4111
wpose.position.z = 0.2168
wpose.orientation.x = -0.7068
wpose.orientation.y = 0.0090
wpose.orientation.z = 0.7073
wpose.orientation.w = 0.0086
self.waypoints.append(deepcopy(wpose))
# self.arm.set_pose_target(wpose)
# Specify default (idle) joint states
self.default_joint_states = self.arm.get_current_joint_values()
Q1 = [1.5713225603103638, -2.0962370077716272, -1.3499930540667933, -1.265561882649557, 1.57, 1.596241985951559]
self.default_joint_states = Q1
# Transition state 1
Q3 = [2.70245099067688, -1.6770857016192835, -1.9628542105304163, -1.0727198759662073, 1.5699867010116577, 2.727403402328491]
# Drop states 1
Q4 = [2.702594757080078, -1.9592016378985804, -2.150653664265768, -0.6029952208148401, 1.569974660873413, 2.7274272441864014]
# Transition states 2 (on top of the dropping box)
Q5 = [3.0016443729400635, -1.4666817823993128, -2.172218624745504, -1.0752008597003382, 1.5701428651809692, 3.0269551277160645]
# Drop states 2
Q6 = [3.001344680786133, -1.8149612585650843, -2.391836706792013, -0.507054630910055, 1.5700825452804565, 3.026643753051758]
# Transition states 3
Q7 = [3.3429105281829834, -1.3315523306476038, -2.277149502431051, -1.106044117604391, 1.5708140134811401, 3.3683338165283203]
# Drop states 3
Q8 = [3.3425633907318115, -1.72536546388735, -2.524234120045797, -0.4651830832110804, 1.5708019733428955, 3.3679027557373047]
self.arm.set_joint_value_target(self.default_joint_states)
# Set the internal state to the current state
self.arm.set_start_state_to_current_state()
plan = self.arm.plan()
sleep(1)
self.arm.execute(plan)
self.transition1_states = Q3
# Specify end states (drop object)
self.drop1_joint_states = Q4
# self.end_joint_states[1] = -1.3705
self.transition2_states = Q5
self.drop2_joint_states = Q6
self.transition3_states = Q7
self.drop3_joint_states = Q8
def test():
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('test', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(2)
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
rightarm_group = moveit_commander.MoveGroupCommander("right_arm")
pose_copy = rightarm_group.get_current_pose().pose
rightarm_pose_target = geometry_msgs.msg.Pose()
rightarm_pose_target.orientation.w = 0
rightarm_pose_target.orientation.x = 1
rightarm_pose_target.orientation.y = 0
rightarm_pose_target.orientation.z = 0
rightarm_pose_target.position.x = 0.06
rightarm_pose_target.position.y = 0.21
rightarm_pose_target.position.z = 0.75
rightarm_group.set_joint_value_target(rightarm_pose_target, "Link23", True)
first_plan = rightarm_group.plan()
print "-----------------------------------size of first_plan"
print len(first_plan.joint_trajectory.points)
if first_plan.joint_trajectory.points:
print "success"
# rightarm_group.execute(first_plan)
listener = tf.TransformListener()
find = False
while not find:
try:
(trans,
rot) = listener.lookupTransform('/base_link', '/Link24',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
print trans
find = True
x = trans[0]
y = trans[1]
z = 1 + trans[2]
leftarm_group = moveit_commander.MoveGroupCommander("left_arm")
#to get the trans in link21 and value in link24
link21x = 0.068898
link21y = 0.067365
link21z = 0.895772
link24x = 0.068870
link24y = 0.063403
link24z = 0.706849
size0 = max(x, link21x, link24x) - min(
x, link21x,
link24x) + 0.04 #[0.04, 0.04, 0.05] is the size of link
size1 = max(y, link21y, link24y) - min(y, link21y, link24y) + 0.04
size2 = max(z, link21z, link24z) - min(z, link21z, link24z) + 0.05
size = (size0, size1, size2)
#set the collision to avoid the leftarm plan to touch the rightarm when they are move Simultaneously
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = leftarm_group.get_planning_frame()
box_pose.pose.orientation.w = 1
box_pose.pose.position.x = x
box_pose.pose.position.y = y
box_pose.pose.position.z = z
scene.add_box("box", box_pose, size)
leftarm_pose_target = geometry_msgs.msg.Pose()
leftarm_pose_target.orientation.w = 0
leftarm_pose_target.orientation.x = 1
leftarm_pose_target.orientation.y = 0
leftarm_pose_target.orientation.z = 0
leftarm_pose_target.position.x = -0.06
leftarm_pose_target.position.y = 0.21
leftarm_pose_target.position.z = 0.75
leftarm_group.set_joint_value_target(leftarm_pose_target, "Link13",
True)
second_plan = leftarm_group.plan()
print "-----------------------------------size of second_plan"
print len(second_plan.joint_trajectory.points)
scene.remove_world_object("box")
if second_plan.joint_trajectory.points:
print "second success"
leftarm_group.execute(second_plan)
else:
rightarm_group.set_joint_value_target(pose_copy, "Link23",
True)
recovery_plan = rightarm_group.plan()
rightarm_group.execute(recovery_plan)
def plan_motion(self, x_start, w_start, x_mid, w_mid, x_final, w_final,
planning_time, left_or_right_eef):
# liangyuwei: This function computes a cartesian path with regard to the specified starting point, via point and the final point.
### Set initial pose
print "== Set initial pose =="
if (left_or_right_eef): # 1 or left eff
group = moveit_commander.MoveGroupCommander("left_arm")
left_pose_target = group.get_current_pose('left_ee_link')
left_pose_target.pose.position.x = x_start[0]
left_pose_target.pose.position.y = x_start[1]
left_pose_target.pose.position.z = x_start[2]
left_pose_target.pose.orientation.x = w_start[0]
left_pose_target.pose.orientation.y = w_start[1]
left_pose_target.pose.orientation.z = w_start[2]
left_pose_target.pose.orientation.w = w_start[3]
group.set_pose_target(left_pose_target, 'left_ee_link')
else:
group = moveit_commander.MoveGroupCommander("right_arm")
right_pose_target = group.get_current_pose('right_ee_link')
right_pose_target.pose.position.x = x_start[0]
right_pose_target.pose.position.y = x_start[1]
right_pose_target.pose.position.z = x_start[2]
right_pose_target.pose.orientation.x = w_start[0]
right_pose_target.pose.orientation.y = w_start[1]
right_pose_target.pose.orientation.z = w_start[2]
right_pose_target.pose.orientation.w = w_start[3]
group.set_pose_target(right_pose_target, 'right_ee_link')
# set planning time
group.set_planning_time(planning_time)
# plan
plan = group.go(wait=True)
# stop
group.stop()
# clear targets
group.clear_pose_targets()
### Set via point
print "== Set via point =="
waypoints = []
wpose = group.get_current_pose().pose
# first mid point
wpose.position.x = x_mid[0] # scale * 0.2
wpose.position.y = x_mid[1] # scale * 0.0 # move to the middle
wpose.position.z = x_mid[2] # scale * 0.315 # minimum height
wpose.orientation.x = w_mid[0]
wpose.orientation.y = w_mid[1]
wpose.orientation.z = w_mid[2]
wpose.orientation.w = w_mid[3]
waypoints.append(copy.deepcopy(wpose))
### Set the final point
print "== Set final pose =="
wpose.position.x = x_final[0]
wpose.position.y = x_final[1]
wpose.position.z = x_final[2]
wpose.orientation.x = w_final[0]
wpose.orientation.y = w_final[1]
wpose.orientation.z = w_final[2]
wpose.orientation.w = w_final[3]
waypoints.append(copy.deepcopy(wpose))
### Compute a cartesian path
print "== Compute a cartesian path =="
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.001, #0.01, # eef_step # set to 0.001 for collecting the data
0.0, # jump_threshold
avoid_collisions=False)
### Execute the plan
print "== Execute the plan =="
self.execute_plan(plan)
return plan
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("manipulator")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(10)
print "============ Starting tutorial "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
## Now, we call the planner to compute the plan
## and visualize it if successful
## Note that we are just planning, not asking move_group
## to actually move the robot
plan1 = group.plan()
print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)
## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the
## group.plan() method does this automatically so this is not that useful
## here (it just displays the same trajectory again).
print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory)
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
## Moving to a pose goal
## ^^^^^^^^^^^^^^^^^^^^^
##
## Moving to a pose goal is similar to the step above
## except we now use the go() function. Note that
## the pose goal we had set earlier is still active
## and so the robot will try to move to that goal. We will
## not use that function in this tutorial since it is
## a blocking function and requires a controller to be active
## and report success on execution of a trajectory.
# Uncomment below line when working with a real robot
# group.go(wait=True)
## Planning to a joint-space goal
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
##
## Let's set a joint space goal and move towards it.
## First, we will clear the pose target we had just set.
group.clear_pose_targets()
## Then, we will get the current set of joint values for the group
group_variable_values = group.get_current_joint_values()
print "============ Joint values: ", group_variable_values
## Now, let's modify one of the joints, plan to the new joint
## space goal and visualize the plan
group_variable_values[0] = 1.0
group.set_joint_value_target(group_variable_values)
plan2 = group.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a cartesian path directly by specifying a list of waypoints
## for the end-effector to go through.
waypoints = []
# start with the current pose
waypoints.append(group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))
# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))
# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def main():
rospy.init_node("terminal_control")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.1)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
x_pos = 0.2
y_pos = 0.0
z_pos = 0.3
x_ang = -3.14
y_ang = 0.0
z_ang = -3.14 / 2.0
grip_ang = 0.7
while (True):
print("Arm current pose:")
print(arm.get_current_pose().pose)
input_mode = raw_input(
"Position (p), Orientation (o) or both (b)? Toggle Gripper (t). Otherwise, quit (q) "
)
if (input_mode is not "p" and input_mode is not "o"
and input_mode is not "b" and input_mode is not "q"
and input_mode is not "t"):
print("Try again")
continue
elif input_mode is "q":
print("Exiting command mode, shutting down...")
break
if input_mode is "p" or input_mode is "b":
while (True): #keep trying until get 3 valid points
try:
x_pos = float(
raw_input(
"Cartesian Coordinate (x) in metres from base: "))
y_pos = float(
raw_input(
"Cartesian Coordinate (y) in metres from base: "))
z_pos = float(
raw_input(
"Cartesian Coordinate (z) in metres from base: "))
break
except:
print("Enter valid float values, please")
pass
if input_mode is "o" or input_mode is "b":
while (True): #keep trying until get 3 valid points
try:
x_ang = float(raw_input("Angle about x-axis in radians: "))
y_ang = float(raw_input("Angle about y-axis in radians: "))
z_ang = float(raw_input("Angle about z-axis in radians: "))
break
except:
print("Enter valid float values, please")
pass
if (input_mode is not "t"):
# 掴む準備をする
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = x_pos #0.2
target_pose.position.y = y_pos #0.0
target_pose.position.z = z_pos #0.3
q = quaternion_from_euler(
x_ang, y_ang, z_ang) #(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
elif input_mode is "t":
# ハンドを開く
grip_ang = 0.7 if grip_ang < 0.7 else 0.3
gripper.set_joint_value_target([grip_ang, grip_ang]) #([0.7, 0.7])
gripper.go()
print("done")
print("shutting down the arm...")
def __init__(self):
# The current status of the joints.
self.JointState = JointTrajectoryControllerState()
# The servo power's status of the robot.
self.ServoPowerState = Bool()
# The fault power's status of the robot.
self.PowerFaultState = Bool()
# The reference coordinate in the calculations of the elfin_basic_api node
self.RefCoordinate = String()
# The end coordinate in the calculations of the elfin_basic_api node
self.EndCoordinate = String()
#The value of the dynamic parameters of elfin_basic_api, e.g. velocity scaling.
self.DynamicArgs = Config()
# get the reference coordinate name of elfin_basic_api from the response of this service.
self.call_ref_coordinate = rospy.ServiceProxy('elfin_basic_api/get_reference_link', SetBool)
self.call_ref_coordinate_req = SetBoolRequest()
# get the current position of elfin_ros_control from the response of this service.
self.call_current_position = rospy.ServiceProxy('elfin_ros_control/elfin/get_current_position', SetBool)
self.call_current_position_req = SetBoolRequest()
# call service recognize_position of elfin_ros_control.
self.call_recognize_position = rospy.ServiceProxy('elfin_ros_control/elfin/recognize_position', SetBool)
self.call_recognize_position_req = SetBoolRequest()
self.call_recognize_position_req.data = True
# get the end coordinate name of elfin_basic_api from the response of this service.
self.call_end_coordinate = rospy.ServiceProxy('elfin_basic_api/get_end_link', SetBool)
self.call_end_coordinate_req = SetBoolRequest()
# for publishing joint goals to elfin_basic_api
self.JointsPub = rospy.Publisher('elfin_basic_api/joint_goal', JointState, queue_size=1)
self.JointsGoal = JointState()
# for publishing cart goals to elfin_basic_api
self.CartGoalPub = rospy.Publisher('elfin_basic_api/cart_goal', PoseStamped, queue_size=1)
self.CartPos = PoseStamped()
# for pub cart path
self.CartPathPub = rospy.Publisher('elfin_basic_api/cart_path_goal', PoseArray, queue_size=1)
self.CartPath = PoseArray()
self.CartPath.header.stamp=rospy.get_rostime()
self.CartPath.header.frame_id='elfin_base_link'
# for pub one specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointCmdPub = rospy.Publisher('elfin_teleop_joint_cmd_no_limit', Int64 , queue_size=1)
self.JointCmd = Int64()
# for pub multi specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointsCmdPub = rospy.Publisher('changyuan_joints_cmd', JointsFloat64, queue_size=1)
self.JointsCmd = JointsFloat64()
# action client, send goal to move_group
self.action_client = SimpleActionClient('elfin_module_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal = FollowJointTrajectoryGoal()
#self.goal_list = JointTrajectoryPoint()
self.goal_list = []
self.joints_ = []
self.ps_ = []
self.listener = tf.TransformListener()
self.robot=moveit_commander.RobotCommander()
self.scene=moveit_commander.PlanningSceneInterface()
self.group=moveit_commander.MoveGroupCommander('elfin_arm')
self.ref_link_name=self.group.get_planning_frame()
self.end_link_name=self.group.get_end_effector_link()
self.ref_link_lock=threading.Lock()
self.end_link_lock=threading.Lock()
self.call_teleop_stop=rospy.ServiceProxy('elfin_basic_api/stop_teleop', SetBool)
self.call_teleop_stop_req=SetBoolRequest()
self.call_teleop_joint=rospy.ServiceProxy('elfin_basic_api/joint_teleop',SetInt16)
self.call_teleop_joint_req=SetInt16Request()
self.call_teleop_joints=rospy.ServiceProxy('elfin_basic_api/joints_teleops',SetFloat64s)
self.call_teleop_joints_req=SetFloat64sRequest()
self.call_teleop_cart=rospy.ServiceProxy('elfin_basic_api/cart_teleop', SetInt16)
self.call_teleop_cart_req=SetInt16Request()
self.call_move_homing=rospy.ServiceProxy('elfin_basic_api/home_teleop', SetBool)
self.call_move_homing_req=SetBoolRequest()
self.call_reset=rospy.ServiceProxy(self.elfin_driver_ns+'clear_fault', SetBool)
self.call_reset_req=SetBoolRequest()
self.call_reset_req.data=True
self.call_power_on = rospy.ServiceProxy(self.elfin_driver_ns+'enable_robot', SetBool)
self.call_power_on_req=SetBoolRequest()
self.call_power_on_req.data=True
self.call_power_off = rospy.ServiceProxy(self.elfin_driver_ns+'disable_robot', SetBool)
self.call_power_off_req = SetBoolRequest()
self.call_power_off_req.data=True
self.call_velocity_setting = rospy.ServiceProxy('elfin_basic_api/set_velocity_scale', SetFloat64)
self.call_velocity_req = SetFloat64Request()
self._velocity_scale = 0.78
self.set_velocity_scale(self._velocity_scale)
pass
print "============ Starting setup"
Force_Measurement = 0
P = [[1.8288, -0.0447, 1.237]]
Q = [0.718181636243, -0.0836401543762, 0.687115714468, 0.0713544453462]
Robot_Pos = []
Robot_Joint = []
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('collision_checker',
'move_group_python_interface_tutorial',
anonymous=True)
## MoveIt! Initialization
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("move_group")
group.set_goal_position_tolerance(0.04)
group.allow_replanning(True)
group.set_planner_id(
"RRTConnectkConfigDefault"
) #RRTConnectkConfigDefault/SBLkConfigDefault/KPIECEkConfigDefault/BKPIECEkConfigDefault/LBKPIECEkConfigDefault/
group.set_num_planning_attempts(5)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
rospy.sleep(2)
print "============ Printing robot Pose"
print group.get_current_pose().pose
tic = timeit.default_timer()
dt = 0
def __init__(self):
### MoveIt!
moveit_commander.roscpp_initialize(sys.argv)
br = tf.TransformBroadcaster()
#rospy.init_node('move_group_planner',
# anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.plan_scene = moveit_commander.PlanningScene()
self.group_1st = moveit_commander.MoveGroupCommander("panda_left")
self.group_2nd = moveit_commander.MoveGroupCommander("panda_right")
self.group_3rd = moveit_commander.MoveGroupCommander("panda_top")
self.group_list = [self.group_1st, self.group_2nd, self.group_3rd]
self.group_chain = moveit_commander.MoveGroupCommander(
"panda_closed_chain")
self.group_chairup = moveit_commander.MoveGroupCommander(
"panda_chair_up")
self.hand_left = moveit_commander.MoveGroupCommander("hand_left")
self.hand_right = moveit_commander.MoveGroupCommander("hand_right")
self.hand_top = moveit_commander.MoveGroupCommander("hand_top")
self.hand_list = [self.hand_left, self.hand_right, self.hand_top]
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group_1st.get_planning_frame()
print("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group_1st.get_end_effector_link()
print("============ End effector: %s" % self.eef_link)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group_3rd.get_planning_frame()
print("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group_3rd.get_end_effector_link()
print("============ End effector: %s" % self.eef_link)
# We can get a list of all the groups in the robot:
self.group_names = self.robot.get_group_names()
print("============ Robot Groups:", self.robot.get_group_names())
rospy.sleep(1)
self.stefan = SceneObject()
# self.scene.remove_attached_object(self.group_1st.get_end_effector_link())
# self.scene.remove_attached_object(self.group_3rd.get_end_effector_link())
# self.scene.remove_world_object()
### Franka Collision
self.set_collision_behavior = rospy.ServiceProxy(
'franka_control/set_force_torque_collision_behavior',
franka_control.srv.SetForceTorqueCollisionBehavior)
#self.set_collision_behavior.wait_for_service()
self.active_controllers = []
self.listener = tf.TransformListener()
self.tr = TransformerROS()
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "base"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.x = 0.8
box_pose.pose.position.z = 0.675
box_pose2 = geometry_msgs.msg.PoseStamped()
box_pose2.header.frame_id = "base"
box_pose2.pose.orientation.w = 1.0
box_pose2.pose.position.x = 0.8
box_pose2.pose.position.y = -0.25
box_pose2.pose.position.z = 1.0
self.scene.add_box("box", box_pose, size=(1.2, 0.5, 0.15))
