def __init__(self, pose):
self.check_collision = False
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 将场景中的颜色设置发布
self.sendColors()
rospy.sleep(5)
joint_positions = pose
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
if arm.go():
pass
else:
self.check_collision = True
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('start_pos')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def execute(self, userdata):
mc = MoveGroupCommander("ext")
mc.set_joint_value_target("ext_axis", userdata.position)
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
class HeadMover():
""" Moves head to specified joint values """
def __init__(self):
self.group_head = MoveGroupCommander('head')
def move_head(self, name, joint_values):
rospy.loginfo('Moving head to specified position')
self.group_head.set_joint_value_target(joint_values)
self.group_head.go()
def execute(self, userdata):
goal = userdata.goal
mc = MoveGroupCommander("r" + str(userdata.robot) + "_arm")
for joint in goal.__slots__:
mc.set_joint_value_target("r" + str(userdata.robot) + "_joint_" + str(joint), getattr(goal, joint))
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
def main():
init_node()
# Preparing Head
rospy.loginfo("Preparing Head...")
headg = MoveGroupCommander("head")
headg.set_joint_value_target([0.0, 60.0 / 180.0 * math.pi])
headg.go()
# Preparing Both Arms
rospy.loginfo("Preparing Left Arm...")
barmg = MoveGroupCommander("botharms")
barmg.set_pose_target([0.325, 0.482, 0.167, 0.0, -math.pi / 2, 0.0],
'LARM_JOINT5_Link')
barmg.set_pose_target([0.325, -0.482, 0.167, 0.0, -math.pi / 2, 0.0],
'RARM_JOINT5_Link')
barmg.go()
rospy.sleep(2.0)
# Right Arm
group = MoveGroupCommander("right_arm")
# Frame ID Definitoins
planning_frame_id = group.get_planning_frame()
tgt_frame_id = '/ar_marker_4'
# Get a target pose
pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id, 5.0)
# Move to a point above target
if pose_target:
# Rotate Pose for Right Hand
quat = []
quat.append(pose_target.pose.orientation.x)
quat.append(pose_target.pose.orientation.y)
quat.append(pose_target.pose.orientation.z)
quat.append(pose_target.pose.orientation.w)
quat = quaternion_multiply(
quat, quaternion_about_axis(math.pi / 2, (1, 0, 0)))
quat = quaternion_multiply(
quat, quaternion_about_axis(math.pi / 2, (0, 0, 1)))
pose_target.pose.orientation.x = quat[0]
pose_target.pose.orientation.y = quat[1]
pose_target.pose.orientation.z = quat[2]
pose_target.pose.orientation.w = quat[3]
pose_target.pose.position.z += 0.4
rospy.loginfo("Set Target To: \n{}".format(pose_target))
group.set_pose_target(pose_target)
ret = group.go()
rospy.loginfo("Executed ... {}".format(ret))
else:
rospy.logwarn("Pose Error: {}".format(pose_target))
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_speed_demo')
# Initialize the move group for the right arm
arm = MoveGroupCommander('manipulator')
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
# Start the arm in the "home" pose stored in the SRDF file
arm.set_named_target('home')
arm.go()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
# Start the arm in the "home" pose stored in the SRDF file
arm.set_named_target('home')
arm.go()
# Get back the planned trajectory
arm.set_joint_value_target(joint_positions)
traj = arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
arm.execute(new_traj)
rospy.sleep(1)
arm.set_named_target('home')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def execute(self, userdata):
goal_r1 = MA1400JointState()
goal_r2 = MA1400JointState()
mc = MoveGroupCommander("arms")
for joint in goal_r1.__slots__:
mc.set_joint_value_target("r1_joint_" + str(joint), getattr(goal_r1, joint))
for joint in goal_r2.__slots__:
mc.set_joint_value_target("r2_joint_" + str(joint), getattr(goal_r2, joint))
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
class MoveitCommanderMoveGroupState(EventState):
"""
Uses moveit commander to plan to the given joint configuration and execute the resulting trajctory.
"""
def __init__(self, planning_group, joint_names):
"""Constructor"""
super(MoveitCommanderMoveGroupState,
self).__init__(outcomes=['reached', 'failed'],
input_keys=['target_joint_config'])
self._planning_group = planning_group
self._joint_names = joint_names
Logger.loginfo("About to make mgc in init with group %s" %
self._planning_group)
self.group_to_move = MoveGroupCommander(self._planning_group)
Logger.loginfo("finished making mgc in init.")
self._done = False
def execute(self, userdata):
"""Execute this state"""
if self._done is not False:
return self._done
def on_enter(self, userdata):
# create the motion goal
Logger.loginfo("Entering MoveIt Commander code!")
if len(self._joint_names) != len(userdata.target_joint_config):
Logger.logwarn(
"Number of joint names (%d) does not match number of joint values (%d)"
% (len(self._joint_names), len(userdata.target_joint_config)))
self.group_to_move.set_joint_value_target(
dict(zip(self._joint_names, userdata.target_joint_config)))
# execute the motion
try:
Logger.loginfo("Moving %s to: %s" %
(self._planning_group, ", ".join(
map(str, userdata.target_joint_config))))
result = self.group_to_move.go()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\n%s' %
str(e))
self._done = "failed"
if result:
self._done = "reached"
else:
self._done = "failed"
class MoveitCommanderMoveGroupState(EventState):
"""
Uses moveit commander to plan to the given joint configuration and execute the resulting trajctory.
"""
def __init__(self, planning_group, joint_names):
"""Constructor"""
super(MoveitCommanderMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
input_keys=['target_joint_config'])
self._planning_group = planning_group
self._joint_names = joint_names
Logger.loginfo("About to make mgc in init with group %s" % self._planning_group)
self.group_to_move = MoveGroupCommander(self._planning_group)
Logger.loginfo("finished making mgc in init.")
self._done = False
def execute(self, userdata):
"""Execute this state"""
if self._done is not False:
return self._done
def on_enter(self, userdata):
# create the motion goal
Logger.loginfo("Entering MoveIt Commander code!")
if len(self._joint_names) != len(userdata.target_joint_config):
Logger.logwarn("Number of joint names (%d) does not match number of joint values (%d)"
% (len(self._joint_names), len(userdata.target_joint_config)))
self.group_to_move.set_joint_value_target(dict(zip(self._joint_names, userdata.target_joint_config)))
# execute the motion
try:
Logger.loginfo("Moving %s to: %s" % (self._planning_group, ", ".join(map(str, userdata.target_joint_config))))
result = self.group_to_move.go()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
self._done = "failed"
if result:
self._done = "reached"
else:
self._done = "failed"
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_position', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# max planning time
arm.set_planning_time(10)
# start pose
arm.set_start_state_to_current_state()
end_effector = arm.get_end_effector_link()
rospy.sleep(1)
print "======== create 100 new goal position ========"
start_pose = PoseStamped()
start_pose.header.frame_id = arm.get_planning_frame()
i = 1
while i <= 1000:
# random position
start_pose = arm.get_random_pose(end_effector)
q = quaternion_from_euler(0.0, 0.0, 0.0)
start_pose.pose.orientation = Quaternion(*q)
print "====== move to position", i, "======="
# KDL
arm.set_joint_value_target(start_pose, True)
# IK Fast
# arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y,
# start_pose.pose.position.z], end_effector)
i += 1
arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('gripper_test')
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
arm.set_max_velocity_scaling_factor(0.3)
arm.set_max_acceleration_scaling_factor(0.5)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander('gripper')
self.gripper = gripper
# 控制机械臂先回到初始位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(0.5)
# 控制机械臂到目标位置
arm.set_named_target('test_2')
arm.go()
# 控制夹爪闭合
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
# 控制机械臂先回到初始位置
arm.set_named_target('home')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def set_joint_value_target(self, arg1, arg2=None, arg3=None):
"""HotFix: See MoveGroupCommander for info."""
if (type(arg1) is PoseStamped) or (type(arg1) is Pose):
approx = False
eef = ""
if arg2 is not None:
if type(arg2) is str:
eef = arg2
else:
if type(arg2) is bool:
approx = arg2
else:
raise MoveItCommanderException("Unexpected type")
if arg3 is not None:
if type(arg3) is str:
eef = arg3
else:
if type(arg3) is bool:
approx = arg3
else:
raise MoveItCommanderException("Unexpected type")
# There is a problem that when the r1_ee or r2_ee are specified as
# eef the end effector ends up 0.1 m below the target position to
# fix this we first transform the pose to the tip_link and then call
# MoveGroupCommander
if self._ik_use_tip_link:
if type(arg1) is PoseStamped:
p, eef = self._fix_link(arg1.pose, eef)
arg1.pose = p
else:
arg1, eef = self._fix_link(arg1, eef)
MoveGroupCommander.set_joint_value_target(self, arg1, eef, approx)
else:
MoveGroupCommander.set_joint_value_target(self, arg1, arg2, arg3)
class PickAndPlace:
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self, traj, time, positions, velocities=None):
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions = copy.deepcopy(positions)
if velocities is not None:
point.velocities = copy.deepcopy(velocities)
point.time_from_start = rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self, q_traj, t_traj):
assert (len(q_traj) == len(t_traj))
#Insert current position to beginning.
if t_traj[0] > 1.0e-2:
t_traj.insert(0, 0.0)
q_traj.insert(0, self.Q(arm=arm))
self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj)
def QTrajToDQTraj(self, q_traj, t_traj):
dof = len(q_traj[0])
#Modeling the trajectory with spline.
splines = [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj = []
for t in t_traj:
dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
#print dq_traj
return dq_traj
def JointNames(self):
#0arm= 0
return self.joint_names[0]
def ROSGetJTP(self, q, t, dq=None):
jp = trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions = q
jp.time_from_start = rospy.Duration(t)
if dq is not None: jp.velocities = dq
return jp
def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None):
assert (len(q_traj) == len(t_traj))
if dq_traj is not None: (len(dq_traj) == len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names = joint_names
if dq_traj is not None:
self.traj.points = [
self.ROSGetJTP(q, t, dq)
for q, t, dq in zip(q_traj, t_traj, dq_traj)
]
else:
self.traj.points = [
self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj)
]
self.traj.header.stamp = rospy.Time.now()
#print self.traj
return self.traj
def SmoothQTraj(self, q_traj):
if len(q_traj) == 0: return
q_prev = np.array(q_traj[0])
q_offset = np.array([0] * len(q_prev))
for q in q_traj:
q_diff = np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d] < -math.pi: q_offset[d] += 1
elif q_diff[d] > math.pi: q_offset[d] -= 1
q_prev = copy.deepcopy(q)
q[:] = q + q_offset * 2.0 * math.pi
def add_target(self, target_pose, target_size, frame, x, y, o1, o2, o3,
o4):
target_pose.header.frame_id = frame
target_pose.pose.position.x = x
target_pose.pose.position.y = y
target_pose.pose.position.z = self.table_ground + self.table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.x = o1
target_pose.pose.orientation.y = o2
target_pose.pose.orientation.z = o3
target_pose.pose.orientation.w = o4
#self.scene.add_box(f_target_id,f_target_pose,f_target_size)
def cts(self, start_pose, mid_pose, end_pose, maxtries, exe_signal=False):
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
if mid_pose != 0:
waypoints.append(mid_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
for i in range(2, len(plan.joint_trajectory.points)):
q_traj.append(
plan.joint_trajectory.points[i].positions)
t_traj.append(t_traj[-1] + 0.07)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = 1
return signal, end_joint_state
#move and rotate
def cts_rotate(self,
start_pose,
end_pose,
angle_r,
maxtries,
exe_signal=False):
angle = angle_r * 3.14 / 180.0
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0.0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
per_angle = angle / (len(plan.joint_trajectory.points) - 2)
for i in range(2, len(plan.joint_trajectory.points)):
joint_inc_list = [
j
for j in plan.joint_trajectory.points[i].positions
]
#plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
joint_inc_list[6] -= per_angle * (i - 1)
q_traj.append(joint_inc_list)
t_traj.append(t_traj[-1] + 0.05)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = 1
return signal, end_joint_state
# shaking function:
# freq : shaking freqence
# times : shaking time per action
def shaking(self, initial_state, end_joint_state, freq, times):
q_traj = [initial_state]
t_traj = [0.0]
for i in range(times):
q_traj.append(end_joint_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
q_traj.append(initial_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(6)
#shaking_a: vertical bottle axis
def shake_a(self, pre_p_angle, r_angle, amp):
start_shake_pose = self.arm.get_current_pose(
self.arm_end_effector_link) # for trajectory of shaking
start_joint_state = self.arm.get_current_joint_values(
) # for state[6] to rotate
shift_pose = deepcopy(start_shake_pose)
r_angle = (r_angle / 180.0) * 3.14
pre_p_angle = (pre_p_angle / 180.0) * 3.14
shift_pose.pose.position.x += amp * math.sin(r_angle) * math.cos(
pre_p_angle) # in verticle direction
shift_pose.pose.position.y -= amp * math.sin(r_angle) * math.sin(
pre_p_angle)
shift_pose.pose.position.z += amp * math.cos(r_angle) #...
signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
return signal, end_joint_state
def shake_b(self, pre_p_angle, r_angle, amp):
start_shake_pose = self.arm.get_current_pose(
self.arm_end_effector_link) # for trajectory of shaking
start_joint_state = self.arm.get_current_joint_values(
) # for state[6] to rotate
shift_pose = deepcopy(start_shake_pose)
r_angle = (r_angle / 180.0) * 3.14
pre_p_angle = (pre_p_angle / 180.0) * 3.14
shift_pose.pose.position.x -= amp * math.cos(r_angle) * math.cos(
pre_p_angle) # in verticle direction
shift_pose.pose.position.y += amp * math.cos(r_angle) * math.sin(
pre_p_angle)
shift_pose.pose.position.z += amp * math.sin(r_angle) #...
signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
return signal, end_joint_state
def setupSence(self):
r_tool_size = [0.03, 0.02, 0.18]
l_tool_size = [0.03, 0.02, 0.18]
#real scene table
table_pose = PoseStamped()
table_pose.header.frame_id = self.reference_frame
table_pose.pose.position.x = -0.0
table_pose.pose.position.y = 0.65
table_pose.pose.position.z = self.table_ground + self.table_size[
2] / 2.0
table_pose.pose.orientation.w = 1.0
self.scene.add_box(self.table_id, table_pose, self.table_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = self.arm_end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.057
l_p.pose.position.z = 0.09
l_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p,
l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = self.arm_end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.057
r_p.pose.position.z = 0.09
r_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p,
r_tool_size)
#Params: pose of bottle, grasp_radius, grasp_height, grasp_theta
def get_grasp_pose(self, pose, r, theta):
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = pose.pose.position.x + r * math.sin(theta)
g_p.pose.position.y = pose.pose.position.y - r * math.cos(theta)
g_p.pose.position.z = pose.pose.position.z
g_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
g_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
math.sin(0.5 * theta))
g_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
g_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
math.cos(0.5 * theta))
return g_p
def get_pour_pose(self, pose, h, r, theta):
p_p = PoseStamped()
p_p.header.frame_id = self.arm_end_effector_link
p_p.pose.position.x = pose.pose.position.x - r * math.cos(theta)
p_p.pose.position.y = pose.pose.position.y + r * math.sin(theta)
p_p.pose.position.z = pose.pose.position.z + h
theta *= -1
p_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
p_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
math.sin(0.5 * theta))
p_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
p_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
math.cos(0.5 * theta))
return p_p
def pour_rotate(self, angle_pre, angle_r, r, maxtries):
angle_pre = (angle_pre / 180.0) * 3.14
angle_r_1 = (angle_r / 180.0) * 3.14
initial_state = self.arm.get_current_joint_values()
initial_pose = self.arm.get_current_pose(self.arm_end_effector_link)
final_pose = deepcopy(initial_pose)
final_pose.pose.position.x += r * (
1 - math.cos(angle_r_1)) * math.cos(angle_pre)
final_pose.pose.position.y += r * (
1 - math.cos(angle_r_1)) * math.sin(angle_pre)
final_pose.pose.position.z += r * math.sin(angle_r_1)
signal, e_j_s = self.cts_rotate(initial_pose, final_pose, angle_r,
maxtries, True)
return signal
def pg_g_pp(self, pose, r, pre_r):
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
p_i_radian = np.arctan(abs(pose.pose.position.x /
pose.pose.position.y))
p_i_angle = p_i_radian * 180.0 / 3.14
pick_list = [
0.0, p_i_angle, 5.0, 25.0, 45.0, 65.0, 15.0, 35.0, 55.0, 75.0,
10.0, 20.0, 30.0, 40.0, 50.0, 60.0
]
for i in range(len(pick_list)):
theta = (pick_list[i] / 180.0) * 3.14
if pose.pose.position.x > 0:
theta *= -1.0
pre_grasp_pose = self.get_grasp_pose(pose, r + pre_r, theta)
#pick up
grasp_pose = pre_grasp_pose
grasp_pose.pose.position.x -= pre_r * math.sin(theta)
grasp_pose.pose.position.y += pre_r * math.cos(theta)
signal, e_j_s = self.cts(start_pose, pre_grasp_pose, grasp_pose,
300, True)
if signal == 0: continue
break
rospy.sleep(1)
self.gripperCtrl(0)
rospy.sleep(2)
#move to ori_pose
current_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e_j_s = self.cts(current_pose, pre_grasp_pose, start_pose, 300,
True)
rospy.sleep(2)
rospy.loginfo("Grasping done.")
return start_pose, pre_grasp_pose, grasp_pose
def pp_ps(self, target_pose, pour_angle, r_pour, h_pour):
for i in range(len(pour_angle)):
maxtries = 300
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
theta = (pour_angle[i] / 180.0) * 3.14
pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta)
#move to pose
signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0,
maxtries, True)
if signal_1 == 0: continue
pre_pour_angle = pour_angle[i]
rospy.loginfo("Ready for pouring.")
return pre_pour_angle
def go_back(self, ori_pose, pre_grasp_pose, grasp_pose):
cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e1 = self.cts(cur_pose, 0, ori_pose, 300, True)
rospy.loginfo("back to pre_grasp pose, ready for placing bottle..")
signal_1, e2 = self.cts(ori_pose, pre_grasp_pose, grasp_pose, 300,
True)
rospy.loginfo("back to grasp pose, open gripper..")
self.gripperCtrl(255)
rospy.sleep(1)
signal_2, e3 = self.cts(grasp_pose, pre_grasp_pose, ori_pose, 300,
True)
rospy.loginfo("back to pre_grasp pose, ready for next kind of source.")
def rotate_back(self, angle):
angle = angle * 3.14 / 180.0
current_j_state = self.arm.get_current_joint_values()
current_j_state[6] += angle + 1.57
self.arm.go()
def last_step_go_back(self, ori_pose):
cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e1 = self.cts(cur_pose, 0, ori_pose, 300, True)
rospy.loginfo("back to last step...")
rospy.sleep(1)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene = PlanningSceneInterface()
pub_traj = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.gripperCtrl = rospy.ServiceProxy(
"/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
self.colors = dict()
rospy.sleep(1)
self.robot = moveit_commander.robot.RobotCommander()
self.arm = MoveGroupCommander('arm')
self.arm.allow_replanning(True)
cartesian = rospy.get_param('~cartesian', True)
self.arm_end_effector_link = self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names = [[]]
self.joint_names[0] = rospy.get_param('controller_joint_names')
self.traj = trajectory_msgs.msg.JointTrajectory()
self.sub_jpc = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#scene planning
self.l_id = 'l_tool'
self.r_id = 'r_tool'
self.table_id = 'table'
self.target1_id = 'target1_object'
self.target2_id = 'target2_object'
self.target3_id = 'target3_object'
self.target4_id = 'target4_object'
self.f_target_id = 'receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
self.scene.remove_world_object(self.target3_id)
self.scene.remove_world_object(self.target4_id)
self.scene.remove_world_object(self.f_target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.table_ground = 0.13
self.table_size = [0.9, 0.6, 0.018]
self.setupSence()
target1_size = [0.035, 0.035, 0.19]
target2_size = target1_size
target3_size = target1_size
target4_size = target1_size
self.f_target_size = [0.2, 0.2, 0.04]
f_target_pose = PoseStamped()
pre_pour_pose = PoseStamped()
target1_pose = PoseStamped()
target2_pose = PoseStamped()
target3_pose = PoseStamped()
target4_pose = PoseStamped()
joint_names = [
'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
]
joint_names = rospy.get_param('controller_joint_names')
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = joint_names
#final target
#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
self.add_target(f_target_pose, self.f_target_size,
self.reference_frame, 0.2, 0.6, 0, 0, 0, 1)
self.scene.add_box(self.f_target_id, f_target_pose, self.f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#target localization
#msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
self.add_target(target1_pose, target1_size, self.reference_frame,
-0.25, 0.8, 0, 0, 0, 1)
self.scene.add_box(self.target1_id, target1_pose, target1_size)
self.add_target(target2_pose, target2_size, self.reference_frame,
-0.12, 0.87, 0, 0, 0, 1)
self.scene.add_box(self.target2_id, target2_pose, target2_size)
self.add_target(target3_pose, target3_size, self.reference_frame, 0.02,
0.88, 0, 0, 0, 1)
self.scene.add_box(self.target3_id, target3_pose, target3_size)
self.add_target(target4_pose, target4_size, self.reference_frame, 0.15,
0.80, 0, 0, 0, 1)
self.scene.add_box(self.target4_id, target4_pose, target4_size)
#attach_pose
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
self.setColor(self.target1_id, 0.8, 0, 0, 1.0)
self.setColor(self.target2_id, 0.8, 0, 0, 1.0)
self.setColor(self.target3_id, 0.8, 0, 0, 1.0)
self.setColor(self.target4_id, 0.8, 0, 0, 1.0)
self.setColor(self.f_target_id, 0.8, 0.3, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(3)
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
j_ori_state = [
-1.2628753185272217, -0.442996621131897, -0.1326361745595932,
2.333048105239868, -0.15598002076148987, -1.2167049646377563,
3.1414425373077393
]
#signal= True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
#parameter setup
tar_num = 4
maxtries = 300
r_grasp = 0.15
r_pre_g = 0.1
r_bottle = 0.1
for i in range(0, tar_num):
#grasp target
rospy.loginfo("Choosing source...")
if i == 0:
target_pose = target1_pose
target_id = self.target1_id
target_size = target1_size
amp = 0.1
freq = 2.75
r_angle = 45.0
elif i == 1:
target_pose = target2_pose
target_id = self.target2_id
target_size = target2_size
amp = 0.15
freq = 2.0
r_angle = 30.0
elif i == 2:
target_pose = target3_pose
target_id = self.target3_id
target_size = target3_size
amp = 0.1
freq = 2.75
r_angle = 45.0
elif i == 3:
target_pose = target4_pose
target_id = self.target4_id
target_size = target4_size
amp = 0.1
freq = 2.75
r_angle = 45.0
r_pour = 0.1
h_pour = 0.1
#pour_angle=[15.0,30.0,45.0,60.0,75.0]
pour_angle = [
0.0, -15.0, -10.0, -5.0, 0.0, 10.0, 15.0, 30.0, 45.0, 60.0,
75.0
]
rospy.loginfo("Params loaded.")
rospy.loginfo("Current Source: " + str(i + 1) + " ")
#grasp and back
ori_pose, mid_pose, g_pose = self.pg_g_pp(target_pose, r_grasp,
r_pre_g)
#move to target position for pouring
pre_p_angle = self.pp_ps(f_target_pose, pour_angle, r_pour, h_pour)
rospy.loginfo("pre_pour_angle : " + str(pre_p_angle))
#rotation and shaking
ps_signal = self.pour_rotate(pre_p_angle, r_angle, r_bottle,
maxtries)
if ps_signal != 1:
rospy.loginfo(
"Pre_shaking_rotation failed, back to ori_pose...")
self.last_step_go_back(ori_pose)
rospy.sleep(3)
continue
else:
rospy.loginfo("Shaking planning...")
shake_per_times = 3
shake_times = 0
shake_times_tgt = 1
signal = True
rospy.loginfo("Parameter loaded")
rospy.sleep(1)
signal, end_joint_state = self.shake_a(pre_p_angle, r_angle,
amp)
while signal == 1:
#area_ratio= rospy.wait_for_message('/color_area_ratio',Float64)
#if (area_ratio>ratio_table[i]) or (shake_times>shake_times_tgt):
if (shake_times < shake_times_tgt):
self.shaking(start_joint_state, end_joint_state, freq,
shake_per_times)
shake_times += 1
rospy.loginfo("shaking times : " + str(shake_times) +
" .")
else:
signal = 0
self.rotate_back(r_angle)
self.go_back(ori_pose, mid_pose, g_pose)
rospy.sleep(2)
continue
#remove and shut down
self.scene.remove_attached_object(self.arm_end_effector_link, 'l_tool')
self.scene.remove_attached_object(self.arm_end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1)
scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
#gripper=MoveGroupCommander('gripper')
end_effector_link=arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
#gripper.set_goal_position_tolerance(0.005)
#gripper.set_goal_orientation_tolerance(0.025)
#gripper.allow_replanning(True)
reference_frame='base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id='table'
#cylinder_id='cylinder'
box2_id='box2'
target_id='target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground=0.59
table_size=[0.5,1,0.01]
#box1_size=[0.1,0.05,0.03]
box2_size=[0.15,0.15,0.02]
r_tool_size=[0.05,0.04,0.22]
l_tool_size=[0.05,0.04,0.22]
target_size=[0.05,0.05,0.1]
table_pose=PoseStamped()
table_pose.header.frame_id=reference_frame
table_pose.pose.position.x=0.7
table_pose.pose.position.y=0.0
table_pose.pose.position.z=table_ground+table_size[2]/2.0
table_pose.pose.orientation.w=1.0
scene.add_box(table_id,table_pose,table_size)
'''
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.7
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
'''
box2_pose=PoseStamped()
box2_pose.header.frame_id=reference_frame
box2_pose.pose.position.x=0.55
box2_pose.pose.position.y=-0.12
box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0
box2_pose.pose.orientation.w=1.0
scene.add_box(box2_id,box2_pose,box2_size)
target_pose=PoseStamped()
target_pose.header.frame_id=reference_frame
target_pose.pose.position.x=0.58
target_pose.pose.position.y=0.05
target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=0
target_pose.pose.orientation.w=1
scene.add_box(target_id,target_pose,target_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.06
l_p.pose.position.z=0.11
l_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y= -0.06
r_p.pose.position.z=0.11
r_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size)
#grasp
g_p=PoseStamped()
g_p.header.frame_id=end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y= -0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
self.setColor(table_id,0.8,0,0,1.0)
#self.setColor(box1_id,0.8,0.4,0,1.0)
self.setColor(box2_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor('target_object',0,1,0)
self.sendColors()
#motion planning
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
grasp_pose=target_pose
grasp_pose.pose.position.x-=0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x=0
grasp_pose.pose.orientation.y=0.707
grasp_pose.pose.orientation.z=0
grasp_pose.pose.orientation.w=0.707
#arm.set_start_state_to_current_state()
'''
arm.set_pose_target(grasp_pose,end_effector_link)
traj=arm.plan()
arm.execute(traj)
print arm.get_current_joint_values()
'''
pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415]
arm.set_joint_value_target(pre_joint_state)
traj=arm.plan()
arm.execute(traj)
rospy.sleep(2)
arm.shift_pose_target(0,0.09,end_effector_link)
arm.go()
rospy.sleep(2)
scene.attach_box(end_effector_link,target_id,g_p,target_size)
rospy.sleep(2)
#grasping is over , from now is pouring
arm.shift_pose_target(2,0.15,end_effector_link)
arm.go()
rospy.sleep(2)
joint_state_1=arm.get_current_joint_values()
joint_state_1[0]-=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
joint_state_2=arm.get_current_joint_values()
joint_state_2[6]-=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(1)
#print arm.get_current_joint_values()
#pouring test
for i in range(1,5):
joint_state_2[6]+=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
joint_state_2[6]-=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
print i
joint_state_2[6]+=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(2)
joint_state_1[0]+=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
arm.shift_pose_target(2,-0.15,end_effector_link)
arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link,target_id)
rospy.sleep(2)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link,'l_tool')
rospy.sleep(1)
scene.remove_attached_object(end_effector_link,'r_tool')
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('TCP_Move', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('xarm6')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('link_base')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 角度弧度转换
j1 = 90.0 / 180 * math.pi
j2 = -18.6 / 180 * math.pi
j3 = -28.1 / 180 * math.pi
j4 = 1.0 / 180 * math.pi
j5 = 47.6 / 180 * math.pi
j6 = -0.9 / 180 * math.pi
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [j1, j2, j3, j4, j5, j6]
arm.set_joint_value_target(joint_positions)
arm.go()
rospy.sleep(1)
# 向下按压门把手
current_pose = arm.get_current_joint_values()
current_pose[4] += (20.0 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
#推开门
current_pose = arm.get_current_joint_values()
current_pose[0] -= (42.0 / 180.0) * math.pi
current_pose[1] += (2.0 / 180.0) * math.pi
current_pose[2] -= (11.4 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the arm move group
arm = MoveGroupCommander('arm')
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Set an initial position for the arm
start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]
# Set the goal pose of the end effector to the stored pose
arm.set_joint_value_target(start_position)
# Plan and execute a trajectory to the goal configuration
arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Move end effector to the right 0.3 meters
wpose.position.y -= 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# Move end effector up and back
wpose.position.x -= 0.2
wpose.position.z += 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses
0.025, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# 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.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
rospy.Subscriber("chatter", Float64MultiArray, callback)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
gripper = MoveGroupCommander('gripper')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
gripper.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.2)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置场景物体的名称
table_id = 'table'
# cy_id = 'cy'
box1_id = 'box1'
box2_id = 'box2'
box3_id = 'box3'
sphere_id = 'sphere'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(box3_id)
scene.remove_world_object(sphere_id)
rospy.sleep(1)
#控制机械臂先回到初始化位置
arm.set_named_target('init')
arm.go()
rospy.sleep(1)
# arm.set_named_target('start')
# arm.go()
# rospy.sleep(1)
gripper.set_joint_value_target([0.05])
gripper.go()
rospy.sleep(0)
# 设置桌面的高度
table_ground = 0.37
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.3, 0.01]
box1_size = [0.01, 0.01, 0.19]
box2_size = [0.01, 0.01, 0.19]
box3_size = [0.005, 0.01, 0.3]
sphere_R = 0.01
error = 0.03
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = -table_size[0] / 2.0
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
#scene.add_cylinder
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = -0.09
box1_pose.pose.position.y = table_size[0] / 2.0
box1_pose.pose.position.z = 0.18 + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = -0.09
box2_pose.pose.position.y = -table_size[0] / 2.0
box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# box3_pose = PoseStamped()
# box3_pose.header.frame_id = reference_frame
# box3_pose.pose.position.x = pos_aim[0]
# box3_pose.pose.position.y = pos_aim[1]
# box3_pose.pose.position.z = box3_size[2]/2.0+0.1
# box3_pose.pose.orientation.w = 1.0
# scene.add_box(box3_id, box3_pose, box3_size)
sphere_pose = PoseStamped()
sphere_pose.header.frame_id = reference_frame
sphere_pose.pose.position.x = pos_aim[0] + 0
sphere_pose.pose.position.y = pos_aim[1]
sphere_pose.pose.position.z = pos_aim[2]
sphere_pose.pose.orientation.w = 1.0
scene.add_sphere(sphere_id, sphere_pose, sphere_R)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0, 0, 0, 1)
# self.setColor(table_id, 0.8, 0, 0, 1.0)
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box1_id, 1, 1, 1, 1.0)
self.setColor(box2_id, 1, 1, 1, 1.0)
self.setColor(box3_id, 1, 1, 1, 1.0)
self.setColor(sphere_id, 0.8, 0, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# rospy.INFO("waiting...")
# if(Recive_FLAG==1):
# rospy.INFO("OK！")
# 设置机械臂的运动目标位置
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = pos_aim[0] - error
target_pose.pose.position.y = pos_aim[1]
target_pose.pose.position.z = pos_aim[2]
# target_pose.pose.orientation.w = 1.0
#####0.3
# 0.2 0.2 0.2 0.15 0.15
# 0.12 0.11 0.12 0.15 0.15
# 0.30 0.245 0.35 0.35 0.25
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
gripper.set_joint_value_target([0.03])
gripper.go()
rospy.sleep(1)
# # 控制机械臂终端向x移动5cm
arm.shift_pose_target(0, 0.01, end_effector_link)
arm.go()
rospy.sleep(1)
gripper.set_joint_value_target([0.05])
gripper.go()
rospy.sleep(1)
# # 设置机械臂的运动目标位置，进行避障规划
# target_pose2 = PoseStamped()
# target_pose2.header.frame_id = reference_frame
# target_pose2.pose.position.x = 0.15
# target_pose2.pose.position.y = 0 #-0.25
# target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
# target_pose2.pose.orientation.w = 1.0
# # 控制机械臂运动到目标位置
# arm.set_pose_target(target_pose2, end_effector_link)
# arm.go()
# rospy.sleep(2)
#控制机械臂回到初始化位置
arm.set_named_target('init')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
rospy.spin()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface("base_link")
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Begin the main perception and pick-and-place loop
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the UBR-1 grasp planner as it does not work with our gripper
goal.plan_grasps = False
# Send the goal request to the find_objects action server which will trigger
# the perception pipeline
find_objects.send_goal(goal)
# Wait for a result
find_objects.wait_for_result(rospy.Duration(5.0))
# The result will contain support surface(s) and objects(s) if any are detected
find_result = find_objects.get_result()
# Display the number of objects found
rospy.loginfo("Found %d objects" % len(find_result.objects))
# Remove all previous objects from the planning scene
for name in scene.getKnownCollisionObjects():
scene.removeCollisionObject(name, False)
for name in scene.getKnownAttachedObjects():
scene.removeAttachedObject(name, False)
scene.waitForSync()
# Clear the virtual object colors
scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_size = None
the_object = None
the_object_dist = 1.0
count = -1
# Cycle through all detected objects and keep the nearest one
for obj in find_result.objects:
count += 1
scene.addSolidPrimitive("object%d"%count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Choose the object nearest to the robot
dx = obj.object.primitive_poses[0].position.x - args.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist:
the_object_dist = d
the_object = count
# Get the size of the target
target_size = obj.object.primitives[0].dimensions
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Make sure we found at least one object before setting the target ID
if the_object != None:
target_id = "object%d"%the_object
# Insert the support surface into the planning scene
for obj in find_result.support_surfaces:
# Extend surface to floor
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
2.0, # make table wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# Add to scene
scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
# Get the table dimensions
table_size = obj.primitives[0].dimensions
# If no objects detected, try again
if the_object == None or target_size is None:
rospy.logerr("Nothing to grasp! try again...")
continue
# Wait for the scene to sync
scene.waitForSync()
# Set colors of the table and the object we are grabbing
scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey
scene.sendColors()
# Skip pick-and-place if we are just detecting objects
if args.objects:
if args.once:
exit(0)
else:
continue
# Get the support surface ID
support_surface = find_result.objects[the_object].object.support_surface
# Set the support surface name to the table object
right_arm.set_support_surface_name(support_surface)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = target_pose.pose.position.x
place_pose.pose.position.y = 0.03
place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose half the size of the target to center it in the gripper
try:
grasp_pose.pose.position.x += target_size[0] / 2.0
grasp_pose.pose.position.y -= 0.01
grasp_pose.pose.position.z += target_size[2] / 2.0
except:
rospy.loginfo("Invalid object size so skipping")
continue
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Set the start state to the current state
#right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
rospy.sleep(2)
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
# Give the servos a rest
arbotix_relax_all_servos()
rospy.sleep(2)
if args.once:
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_zero')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_zero')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
#def default_grip(arm):
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
eef = MoveGroupCommander("endeffector")
rospy.sleep(1)
#Start State Gripper
group_variable_values = eef.get_current_joint_values()
group_variable_values[0] = 0.0
eef.set_joint_value_target(group_variable_values)
plan2 = eef.plan()
arm.execute(plan2)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
k = PoseStamped()
k.header.frame_id = robot.get_planning_frame()
k.pose.position.x = 0.0
k.pose.position.y = 0.0
k.pose.position.z = -0.05
scene.add_box("table", k, (2, 2, 0.0001))
p = PoseStamped()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening")]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening")]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral")]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")
# We need a tf listener to convert poses into arm reference base
self.tf_listener = tf.TransformListener()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('target_pose',
PoseStamped,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.04)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 3
# Set a limit on the number of place attempts
max_place_attempts = 3
rospy.loginfo("Scaling for MoveIt timeout=" + str(
rospy.get_param(
'/move_group/trajectory_execution/allowed_execution_duration_scaling'
)))
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
#table_id = 'table' We also remove the table object in order to run a test
#box1_id = 'box1' These boxes are commented as we do not need them
#box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
#scene.remove_world_object(table_id)
#scene.remove_world_object(box1_id) These boxes are commented as we do not need them
#scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "arm_up" pose stored in the SRDF file
rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the closed position
rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed))
gripper.set_joint_value_target(self.gripper_closed)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the neutral position
rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral))
gripper.set_joint_value_target(self.gripper_neutral)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the open position
rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Set the height of the table off the ground
#table_ground = 0.4
# Set the dimensions of the scene objects [l, w, h]
#table_size = [0.2, 0.7, 0.01]
#box1_size = [0.1, 0.05, 0.05] commented for the same reasons as previously
#box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [
0.018, 0.018, 0.018
] #[0.02, 0.005, 0.12] original object dimensions in meters
# Add a table top and two boxes to the scene
#table_pose = PoseStamped()
#table_pose.header.frame_id = REFERENCE_FRAME
#table_pose.pose.position.x = 0.36
#table_pose.pose.position.y = 0.0
#table_pose.pose.position.z = table_ground + table_size[2] -0.08 / 2.0 #0.4+0.01/2 aka table_ground + table_size[2] + target_size[2] / 2.0
#table_pose.pose.orientation.w = 1.0
#scene.add_box(table_id, table_pose, table_size)
#box1_pose = PoseStamped() These two blocks of code are commented as they assign postion to unwanted boxes
#box1_pose.header.frame_id = REFERENCE_FRAME
#box1_pose.pose.position.x = table_pose.pose.position.x - 0.04
#box1_pose.pose.position.y = 0.0
#box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
#box1_pose.pose.orientation.w = 1.0
#scene.add_box(box1_id, box1_pose, box1_size)
#box2_pose = PoseStamped()
#box2_pose.header.frame_id = REFERENCE_FRAME
#box2_pose.pose.position.x = table_pose.pose.position.x - 0.06
#box2_pose.pose.position.y = 0.2
#box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
#box2_pose.pose.orientation.w = 1.0
#scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = -0.03 #table_pose.pose.position.x - 0.03
target_pose.pose.position.y = 0.1
target_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] / 2.0 table_ground + table_size[2] + target_size[2] -0.08 / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
#self.setColor(table_id, 0.8, 0, 0, 1.0)
#self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
#self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
#arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = -0.03 #table_pose.pose.position.x - 0.03
place_pose.pose.position.y = -0.15
place_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] -0.08 / 2.0 0.4+0.01+0.018/2 aka table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id],
[target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
result = arm.pick(target_id, grasps)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo(" Pick: Done!")
# Generate valid place poses
places = self.make_places(place_pose)
success = False #from here we are forcing the success cases by stting and making it always true it makes an error appear and the arm stays to the middle position
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts: #while not has been removed added = after operator <
rospy.loginfo("Place attempt #" + str(n_attempts))
for place in places:
# Publish the place poses so they can be viewed in RViz
self.gripper_pose_pub.publish(place)
rospy.sleep(0.2)
success = arm.place(target_id, place)
break
if success:
break
n_attempts += 1
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " +
str(n_attempts) + " attempts.")
else: #end of forcing
rospy.loginfo(" Place: Done!")
else:
rospy.logerr("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
arm.set_named_target('resting')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_attached_object_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 设置每次运动规划的时间限制：10s
arm.set_planning_time(10)
# 移除场景中之前运行残留的物体
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
# 设置桌面的高度
table_ground = 0.7
# 设置table和tool的三维尺寸
table_size = [0.1, 0.3, 0.02]
tool_size = [0.2, 0.02, 0.02]
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0
p.pose.position.z = -0.015
p.pose.orientation.w = 1
# 将tool附着到机器人的终端
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.4
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2]
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
rospy.sleep(2)
# 更新当前的位姿
arm.set_start_state_to_current_state()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [
-0.5110620856285095, 0.32814791798591614, 0.5454912781715393,
1.0146701335906982, 0.8498637080192566, -0.45695754885673523
]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
rospy.loginfo(str(path))
#waypoints_pa = PoseArray()
#path = RobotTrajectory()
for point in path.joint_trajectory.points: # THIS ONLY SENDS THE MESSAGE FOR THE FIRST JOINT
joint_stt = JointState()
joint_stt.header.frame_id = 'base_link'
joint_stt.header.stamp = rospy.Time.now()
# joint_stt.name.append('arm_right_2_joint')
# joint_stt.position.append(0.5)
# joint_stt.velocity.append(0.0)
joint_stt.name.extend(path.joint_trajectory.joint_names)
joint_stt.position.extend(point.positions)
joint_stt.velocity.extend([0.0]*len(path.joint_trajectory.joint_names))
#joint_stt.effort.append([0.0]*len(path.joint_trajectory.joint_names))
rospy.loginfo("jointstate is: \n" + str(joint_stt))
right_arm_mgc.set_joint_value_target(joint_stt)
right_arm_mgc.go()
# rospy.loginfo("Publishing waypoints in PoseArray /waypoints")
# pub_waypoints = rospy.Publisher('/waypoints', PoseArray, latch=True)
# while True:
# pub_waypoints.publish(waypoints_pa)
# rospy.sleep(0.1)
#rospy.loginfo("go()")
#right_arm_mgc.go()
import rospy
from moveit_commander import MoveGroupCommander
if __name__ == '__main__':
#init_node()
rospy.init_node('message', anonymous=True)
group = MoveGroupCommander("manipulator")
exec_vel = 0.5
while True:
rospy.loginfo("joint1 start")
group.set_max_velocity_scaling_factor(exec_vel)
group.set_joint_value_target([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
group.go()
rospy.loginfo("joint1 end")
rospy.loginfo("pose1 start")
group.set_max_velocity_scaling_factor(exec_vel)
group.set_pose_target([0.5, -0.2, 0.2, 0.0, 1.0, 0.0])
group.go()
rospy.loginfo("pose1 end")
rospy.loginfo("pose2 start")
group.set_max_velocity_scaling_factor(exec_vel)
group.set_pose_target([0.5, -0.2, 0.7, 0.0, 1.0, 0.0])
group.go()
rospy.loginfo("pose2 end")
class RazerControl():
def __init__(self):
self.pub_right_hand_pose = rospy.Publisher(RIGHT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_right_hand_pose_reference = rospy.Publisher(RIGHT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_left_hand_pose = rospy.Publisher(LEFT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_left_hand_pose_reference = rospy.Publisher(LEFT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.hydra_data_subs = rospy.Subscriber(HYDRA_DATA_TOPIC, Hydra, self.hydraDataCallback)
self.pub_move_base = rospy.Publisher(MOVE_BASE_TOPIC, Twist)
self.subs = rospy.Subscriber('/joint_states', JointState, self.getJointStates)
self.current_joint_states = None
rospy.loginfo("Getting first joint_states")
while self.current_joint_states == None:
rospy.sleep(0.1)
rospy.loginfo("Gotten!")
rospy.loginfo("Connecting with right hand AS")
self.right_hand_as = actionlib.SimpleActionClient(HAND_RIGHT_AS, FollowJointTrajectoryAction)
self.right_hand_as.wait_for_server()
rospy.loginfo("Connecting with left hand AS")
self.left_hand_as = actionlib.SimpleActionClient(HAND_LEFT_AS, FollowJointTrajectoryAction)
self.left_hand_as.wait_for_server()
rospy.loginfo("Starting up move group commander for right, left, torso and head... (slow)")
self.right_arm_mgc = MoveGroupCommander("right_arm")
self.right_arm_mgc.set_pose_reference_frame('base_link')
self.left_arm_mgc = MoveGroupCommander("left_arm")
self.left_arm_mgc.set_pose_reference_frame('base_link')
self.torso_mgc = MoveGroupCommander("right_arm_torso")
self.torso_mgc.set_pose_reference_frame('base_link')
self.head_mgc = MoveGroupCommander("head")
self.head_mgc.set_pose_reference_frame('base_link')
self.last_hydra_message = None
self.tmp_pose_right = PoseStamped()
self.tmp_pose_left = PoseStamped()
self.read_message = False
def getJointStates(self, data):
self.current_joint_states = data
def create_hand_goal(self, hand_side="right", hand_pose="closed", values=0.0):
"""Returns the hand goal to send
possible poses: closed, open, intermediate"""
hand_goal = FollowJointTrajectoryGoal()
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_thumb_joint')
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_middle_joint')
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_index_joint')
jtp = JointTrajectoryPoint()
joint_list = ['hand_'+ hand_side +'_thumb_joint',
'hand_'+ hand_side +'_middle_joint',
'hand_'+ hand_side +'_index_joint']
ids_list = []
values_list = []
rospy.loginfo("current_joint_state is:\n" + str(self.current_joint_states))
for joint in joint_list:
idx_in_message = self.current_joint_states.name.index(joint)
ids_list.append(idx_in_message)
values_list.append(self.current_joint_states.position[idx_in_message])
if hand_pose == "closed":
jtp.positions.append(2.0)
jtp.positions.append(values_list[1]) # TODO: read values and keep them
jtp.positions.append(values_list[2]) # TODO: read values and keep them
elif hand_pose == "open":
jtp.positions.append(0.0)
jtp.positions.append(values_list[1]) # TODO: read values and keep them
jtp.positions.append(values_list[2]) # TODO: read values and keep them
elif hand_pose == "intermediate":
jtp.positions.append(values_list[0]) # TODO: read values and keep them
jtp.positions.append(values)
jtp.positions.append(values)
jtp.velocities.append(0.0)
jtp.velocities.append(0.0)
jtp.velocities.append(0.0)
jtp.time_from_start.secs = 2
hand_goal.trajectory.points.append(jtp)
return hand_goal
def hydraDataCallback(self, data):
#rospy.loginfo("Received data from " + HYDRA_DATA_TOPIC)
self.last_hydra_message = data
self.tmp_pose_right = PoseStamped()
self.tmp_pose_right.header.frame_id = 'base_link'
self.tmp_pose_right.header.stamp = rospy.Time.now()
self.tmp_pose_right.pose.position.x = self.last_hydra_message.paddles[1].transform.translation.x
self.tmp_pose_right.pose.position.y = self.last_hydra_message.paddles[1].transform.translation.y
self.tmp_pose_right.pose.position.z = self.last_hydra_message.paddles[1].transform.translation.z
self.tmp_pose_right.pose.position.x += RIGHT_HAND_INITIAL_POINT.x
self.tmp_pose_right.pose.position.y += RIGHT_HAND_INITIAL_POINT.y
self.tmp_pose_right.pose.position.z += RIGHT_HAND_INITIAL_POINT.z
self.tmp_pose_right.pose.orientation = self.last_hydra_message.paddles[1].transform.rotation
self.tmp_pose_left = PoseStamped()
self.tmp_pose_left.header.frame_id = 'base_link'
self.tmp_pose_left.header.stamp = rospy.Time.now()
self.tmp_pose_left.pose.position.x = self.last_hydra_message.paddles[0].transform.translation.x
self.tmp_pose_left.pose.position.y = self.last_hydra_message.paddles[0].transform.translation.y
self.tmp_pose_left.pose.position.z = self.last_hydra_message.paddles[0].transform.translation.z
self.tmp_pose_left.pose.position.x += LEFT_HAND_INITIAL_POINT.x
self.tmp_pose_left.pose.position.y += LEFT_HAND_INITIAL_POINT.y
self.tmp_pose_left.pose.position.z += LEFT_HAND_INITIAL_POINT.z
self.tmp_pose_left.pose.orientation = self.last_hydra_message.paddles[0].transform.rotation
if self.last_hydra_message.paddles[1].buttons[0] == True:
self.pub_right_hand_pose.publish(self.tmp_pose_right)
if self.last_hydra_message.paddles[0].buttons[0] == True:
self.pub_left_hand_pose.publish(self.tmp_pose_left)
self.pub_right_hand_pose_reference.publish(self.tmp_pose_right)
self.pub_left_hand_pose_reference.publish(self.tmp_pose_left)
self.read_message = False
def run(self):
rospy.loginfo("Press LB / RB to send the current pose")
while self.last_hydra_message == None:
rospy.sleep(0.1)
rospy.loginfo("Got the first data of the razer... Now we can do stuff")
sleep_rate=0.05 # check at 20Hz
counter = 0
while True:
counter += 1
rospy.loginfo("Loop #" + str(counter))
if not self.read_message:
self.read_message = True
if self.last_hydra_message.paddles[1].buttons[0] == True:
# send curr left paddle pos to move_group right
rospy.loginfo("sending curr right hand")
self.right_arm_mgc.set_pose_target(self.tmp_pose_right)
self.right_arm_mgc.go(wait=False)
if self.last_hydra_message.paddles[0].buttons[0] == True:
# send curr right paddle pos to move_group left
rospy.loginfo("sending curr left hand")
self.left_arm_mgc.set_pose_target(self.tmp_pose_left)
self.left_arm_mgc.go(wait=False)
if self.last_hydra_message.paddles[1].trigger > 0.0:
# send goal right hand close proportional to trigger value (2.0 max?)
rospy.loginfo("Closing right hand to value: " + str(self.last_hydra_message.paddles[1].trigger * 2.0))
right_hand_goal = self.create_hand_goal(hand_side="right", hand_pose="intermediate", values=self.last_hydra_message.paddles[1].trigger * 2.0)
self.right_hand_as.send_goal(right_hand_goal)
if self.last_hydra_message.paddles[0].trigger > 0.0:
# send goal left hand close proportional to trigger value (2.0 max?)
rospy.loginfo("Closing left hand to value: " + str(self.last_hydra_message.paddles[0].trigger * 2.0))
left_hand_goal = self.create_hand_goal(hand_side="left", hand_pose="intermediate", values=self.last_hydra_message.paddles[0].trigger * 2.0)
self.left_hand_as.send_goal(left_hand_goal)
if self.last_hydra_message.paddles[1].joy[0] != 0.0:
# send torso rotation left(neg)/right (pos)
rospy.loginfo("Rotation torso")
curr_joint_val = self.torso_mgc.get_current_joint_values()
self.torso_mgc.set_joint_value_target("torso_1_joint", curr_joint_val[0] + (self.last_hydra_message.paddles[1].joy[0] * 0.1 * -1))
self.torso_mgc.go(wait=True)
rospy.loginfo("Rotation torso sent!")
if self.last_hydra_message.paddles[1].joy[1] != 0.0:
# send torso inclination front(pos)/back(neg)
rospy.loginfo("Inclination torso")
curr_joint_val = self.torso_mgc.get_current_joint_values()
self.torso_mgc.set_joint_value_target("torso_2_joint", curr_joint_val[1] + (self.last_hydra_message.paddles[1].joy[1] * 0.1))
self.torso_mgc.go(wait=True)
rospy.loginfo("Inclination torso sent!")
if self.last_hydra_message.paddles[0].joy[0] != 0.0 or self.last_hydra_message.paddles[0].joy[1] != 0.0:
twist_goal = Twist()
twist_goal.linear.x = 1.0 * self.last_hydra_message.paddles[0].joy[1]
twist_goal.angular.z = 1.0 * self.last_hydra_message.paddles[0].joy[0] * -1.0
self.pub_move_base.publish(twist_goal)
# move base rotate left (neg)/ right(pos)
rospy.loginfo("Move base")
if self.last_hydra_message.paddles[1].buttons[3] == True:
# thumb up
rospy.loginfo("Right thumb up")
right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="open")
self.right_hand_as.send_goal(right_thumb_up)
if self.last_hydra_message.paddles[0].buttons[3] == True:
# thumb up
rospy.loginfo("Left thumb up")
left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="open")
self.left_hand_as.send_goal(left_thumb_up)
if self.last_hydra_message.paddles[1].buttons[1] == True:
# thumb down
rospy.loginfo("Right thumb down")
right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="closed")
self.right_hand_as.send_goal(right_thumb_up)
if self.last_hydra_message.paddles[0].buttons[1] == True:
# thumb down
rospy.loginfo("Left thumb down")
left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="closed")
self.left_hand_as.send_goal(left_thumb_up)
rospy.sleep(sleep_rate)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(60)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "grasp" pose stored in the SRDF file
right_arm.set_named_target('right_arm_up')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(5)
# Set the height of the table off the ground
table_ground = 0.04
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
target_x = 0.135
#target_y = -0.32
target_y = -0.285290879999
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = target_x
target_pose.pose.position.y = target_y
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = 0
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
p = PoseStamped()
p.header.frame_id = "up1_footprint"
p.pose.position.x = 0.12792118579
p.pose.position.y = -0.285290879999
p.pose.position.z = 0.120301181892
p.pose.orientation.x = 0.0
p.pose.orientation.y = 0.0
p.pose.orientation.z = -0.706825181105
p.pose.orientation.w = 0.707388269167
right_gripper.set_pose_target(p.pose)
# pick an object
right_arm.allow_replanning(True)
right_arm.allow_looking(True)
right_arm.set_goal_tolerance(0.05)
right_arm.set_planning_time(60)
print "arm grasp"
success = 0
attempt = 0
while not success:
p_plan = right_arm.plan()
attempt = attempt + 1
print "Planning attempt: " + str(attempt)
if p_plan.joint_trajectory.points != []:
success = 1
print "arm grasp"
right_arm.execute(p_plan)
rospy.sleep(5)
right_gripper.set_joint_value_target(GRIPPER_GRASP)
right_gripper.go()
print "gripper closed"
rospy.sleep(5)
scene.attach_box(GRIPPER_FRAME, target_id)
print "object attached"
right_arm.set_named_target('right_arm_up')
right_arm.go()
print "arm up"
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.65
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.55
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.54
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.60
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.25
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class PlannerAnnotationParser(AnnotationParserBase):
"""
Parses the annotations files that contains the benchmarking
information.
"""
def __init__(self, path_to_annotation, path_to_data):
super(PlannerAnnotationParser, self).__init__(path_to_annotation,
path_to_data)
self.parse()
self._load_scene()
self._init_planning()
self.benchmark()
def check_results(self, results):
"""
Returns the results from the planner, checking them against any eventual validation data
(no validation data in our case).
"""
return self.planner_data
def _load_scene(self):
"""
Loads the proper scene for the planner.
It can be either a python static scene or bag containing an occupancy map.
"""
scene = self._annotations["scene"]
for element in scene:
if element["type"] == "launch":
self.play_launch(element["name"])
elif element["type"] == "python":
self.load_python(element["name"])
elif element["type"] == "bag":
self.play_bag(element["name"])
for _ in range(150):
rospy.sleep(0.3)
# wait for the scene to be spawned properly
rospy.sleep(0.5)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
joints = test["goal_joints"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
joints,
self._annotations["name"] + "-test_" + str(test_id))
return self.planner_data
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def _plan_joints(self, joints, test_name):
# plan to joint target and determine success
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
plan_time = "N/A"
total_joint_rotation = "N/A"
comp_time = "N/A"
plan_success = self._check_plan_success(plan)
if plan_success:
plan_time = self._check_plan_time(plan)
total_joint_rotation = self._check_plan_total_rotation(plan)
while not self._comp_time:
rospy.sleep(0.5)
comp_time = self._comp_time.pop(0)
self.planner_data.append([
self.planner_id, test_name,
str(plan_success), plan_time, total_joint_rotation, comp_time
])
@staticmethod
def _check_plan_success(plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
@staticmethod
def _check_plan_time(plan):
# find duration of successful plan
number_of_points = len(plan.joint_trajectory.points)
time = plan.joint_trajectory.points[number_of_points -
1].time_from_start.to_sec()
return time
@staticmethod
def _check_plan_total_rotation(plan):
# find total joint rotation in successful trajectory
angles = [0, 0, 0, 0, 0, 0]
number_of_points = len(plan.joint_trajectory.points)
for i in range(number_of_points - 1):
angles_temp = [
abs(x - y)
for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
plan.joint_trajectory.points[i].positions)
]
angles = [x + y for x, y in zip(angles, angles_temp)]
total_angle_change = sum(angles)
return total_angle_change
def _check_computation_time(self, msg):
# get computation time for successful plan to be found
if msg.status.status == 3:
self._comp_time.append(msg.result.planning_time)
def _check_path_length(self, plan):
# find distance travelled by end effector
# not yet implemented
return
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
class DaArmServer:
"""The basic, design problem/tui agnostic arm server
"""
gestures = {}
pointing_height = 0.06
grasp_height = 0.05
drop_height = 0.07
cruising_height = 0.1
START_TOLERANCE = 0.05 # this is for moveit to check for change in joint angles before moving
GOAL_TOLERANCE = 0.005
moving = False
paused = False
move_group_state = "IDLE"
last_joint_trajectory_goal = ""
last_joint_trajectory_result = ""
def __init__(self, num_planning_attempts=100, safe_zone=None):
rospy.init_node("daarm_server", anonymous=True)
self.safe_zone = safe_zone # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}]
self.init_params()
self.init_scene()
self.init_publishers()
self.init_subscribers()
self.init_action_clients()
self.init_service_clients()
self.init_arm(num_planning_attempts)
def init_arm(self, num_planning_attempts=700):
rospy.set_param(
"/move_group/trajectory_execution/allowed_start_tolerance",
self.START_TOLERANCE)
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE)
self.arm.set_goal_orientation_tolerance(0.02)
self.init_services()
self.init_action_servers()
def init_scene(self):
world_objects = [
"table", "tui", "monitor", "overHead", "wall", "farWall",
"frontWall", "backWall", "blockProtector", "rearCamera"
]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8836)
self.overHeadPose = PoseStamped()
self.overHeadPose.header.frame_id = self.robot.get_planning_frame()
self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97)
self.overHeadDimension = (0.9906, 0.8382, 0.05)
self.blockProtectorPose = PoseStamped()
self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame(
)
self.blockProtectorPose.pose.position = Point(
0.0056, -0.343, -0.51 + self.cruising_height)
self.blockProtectorDimension = (0.9906, 0.8382, 0.8636)
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
self.farWallPose = PoseStamped()
self.farWallPose.header.frame_id = self.robot.get_planning_frame()
self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048)
self.farWallDimension = (0.6096, 2, 3.35)
self.frontWallPose = PoseStamped()
self.frontWallPose.header.frame_id = self.robot.get_planning_frame()
self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51)
self.frontWallDimension = (1, 0.15, 4)
self.backWallPose = PoseStamped()
self.backWallPose.header.frame_id = self.robot.get_planning_frame()
self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51)
self.backWallDimension = (1, 0.005, 4)
self.rearCameraPose = PoseStamped()
self.rearCameraPose.header.frame_id = self.robot.get_planning_frame()
self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51)
self.rearCameraDimension = (0.5, 0.5, 2)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
self.scene.add_box("farWall", self.farWallPose, self.farWallDimension)
self.scene.add_box("overHead", self.overHeadPose,
self.overHeadDimension)
self.scene.add_box("backWall", self.backWallPose,
self.backWallDimension)
self.scene.add_box("frontWall", self.frontWallPose,
self.frontWallDimension)
self.scene.add_box("rearCamera", self.rearCameraPose,
self.rearCameraDimension)
def raise_table(self):
#raises the table obstacle to protect blocks on the table during transport
self.scene.remove_world_object("blockProtector")
self.scene.add_box("blockProtector", self.blockProtectorPose,
self.blockProtectorDimension)
def lower_table(self):
#lowers the table to allow grasping into it
self.scene.remove_world_object("blockProtector")
def init_params(self):
try:
self.grasp_height = get_ros_param(
"GRASP_HEIGHT", "Grasp height defaulting to 0.01")
self.drop_height = get_ros_param("DROP_HEIGHT",
"Drop height defaulting to 0.07")
self.cruising_height = get_ros_param(
"CRUISING_HEIGHT", "Cruising height defaulting to 0.1")
self.pointing_height = get_ros_param(
"POINT_HEIGHT", "Pointing height defaulting to 0.06")
except ValueError as e:
rospy.loginfo(e)
def handle_param_update(self, message):
self.init_params()
def init_publishers(self):
self.calibration_publisher = rospy.Publisher("/calibration_results",
CalibrationParams,
queue_size=1)
self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs",
String,
queue_size=1)
rospy.sleep(0.5)
def init_subscribers(self):
self.joint_angle_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/joint_angles', JointAngles,
self.update_joints)
self.joint_trajectory_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/status', GoalStatusArray,
self.update_joint_trajectory_state)
self.joint_trajectory_goal_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal)
self.joint_trajectory_result_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/result',
FollowJointTrajectoryActionResult,
self.update_joint_trajectory_result)
self.finger_position_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/finger_position', FingerPosition,
self.update_finger_position)
self.param_update_subscriber = rospy.Subscriber(
"/param_update", String, self.handle_param_update)
self.moveit_status_subscriber = rospy.Subscriber(
'/move_group/status', GoalStatusArray,
self.update_move_group_status)
self.move_it_feedback_subscriber = rospy.Subscriber(
'/move_group/feedback', MoveGroupActionFeedback,
self.update_move_group_state)
#Topic for getting joint torques
rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles,
self.monitorJointTorques)
#Topic for getting cartesian force on end effector
rospy.Subscriber('/j2s7s300_driver/out/tool_wrench',
geometry_msgs.msg.WrenchStamped,
self.monitorToolWrench)
def init_action_servers(self):
self.calibration_server = actionlib.SimpleActionServer(
"calibrate_arm", CalibrateAction, self.calibrate, auto_start=False)
self.calibration_server.start()
self.move_block_server = actionlib.SimpleActionServer(
"move_block",
MoveBlockAction,
self.handle_move_block,
auto_start=False)
self.move_block_server.start()
#self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
self.move_pose_server = actionlib.SimpleActionServer(
"move_pose",
MovePoseAction,
self.handle_move_pose,
auto_start=False)
self.move_pose_server.start()
def init_services(self):
self.home_arm_service = rospy.Service("/home_arm", ArmCommand,
self.handle_home_arm)
# emergency stop
self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand,
self.handle_stop_arm)
# stop and pause for a bit
self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand,
self.handle_pause_arm)
self.start_arm_service = rospy.Service("/restart_arm", ArmCommand,
self.handle_restart_arm)
def init_action_clients(self):
# Action Client for joint control
joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
self.joint_action_client = actionlib.SimpleActionClient(
joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
rospy.loginfo('Waiting for ArmJointAnglesAction server...')
self.joint_action_client.wait_for_server()
rospy.loginfo('ArmJointAnglesAction Server Connected')
# Service to move the gripper fingers
finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
self.finger_action_client = actionlib.SimpleActionClient(
finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
self.finger_action_client.wait_for_server()
#
def init_service_clients(self):
self.is_simulation = False
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def handle_start_arm(self, message):
return self.restart_arm()
def handle_stop_arm(self, message):
return self.stop_motion()
def handle_pause_arm(self, message):
self.stop_motion(home=True, pause=True)
return str(self.paused)
def handle_restart_arm(self, message):
self.restart_arm()
self.paused = False
return str(self.paused)
def handle_home_arm(self, message):
try:
status = self.home_arm()
return json.dumps(status)
except rospy.ServiceException as e:
rospy.loginfo("Homing arm failed")
def home_arm(self):
# send the arm home
# for now, let's just use the kinova home
#self.home_arm_client()
self.home_arm_kinova()
return "done"
def custom_home_arm(self):
angles_set = [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 539.743164062
]
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angles_set[0]
goal.angles.joint2 = angles_set[1]
goal.angles.joint3 = angles_set[2]
goal.angles.joint4 = angles_set[3]
goal.angles.joint5 = angles_set[4]
goal.angles.joint6 = angles_set[5]
goal.angles.joint7 = angles_set[6]
self.joint_action_client.send_goal(goal)
def home_arm_kinova(self):
"""Takes the arm to the kinova default home if possible
"""
# self.arm.set_named_target("Home")
angles_set = map(np.deg2rad, [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 269.743164062
])
self.arm.clear_pose_targets()
try:
self.arm.set_joint_value_target(angles_set)
except MoveItCommanderException as e:
pass #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles
try:
self.arm.go()
return "successful home"
except:
return "failed to home"
# This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value
def monitorJointTorques(self, torques):
if abs(torques.joint1) > 1:
return
#self.emergency_stop() #Stop arm driver
#rospy.sleep(1.0)
#self.group.stop() #Stop moveit execution
# This callback function monitors the Joint Wrench and stops the current
# execution if the Joint Wrench exceeds certain value
def monitorToolWrench(self, wrenchStamped):
return
#toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2)
##print toolwrench
#if toolwrench > 100:
# self.emergency_stop() # Stop arm driver
def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
finger_max_turn = 6800
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float((finger1_pct / 100.0) * finger_max_turn)
goal.fingers.finger2 = float((finger2_pct / 100.0) * finger_max_turn)
goal.fingers.finger3 = float((finger3_pct / 100.0) * finger_max_turn)
self.finger_action_client.send_goal(goal)
if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
return self.finger_action_client.get_result()
else:
self.finger_action_client.cancel_all_goals()
rospy.loginfo('the gripper action timed-out')
return None
def move_joint_angles(self, angle_set):
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angle_set[0]
goal.angles.joint2 = angle_set[1]
goal.angles.joint3 = angle_set[2]
goal.angles.joint4 = angle_set[3]
goal.angles.joint5 = angle_set[4]
goal.angles.joint6 = angle_set[5]
goal.angles.joint7 = angle_set[6]
self.joint_action_client.send_goal(goal)
if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
return self.joint_action_client.get_result()
else:
print(' the joint angle action timed-out')
self.joint_action_client.cancel_all_goals()
return None
def handle_move_block(self, message):
"""msg format: {id: int,
source: Point {x: float,y: float},
target: Point {x: float, y: float}
"""
print(message)
pick_x = message.source.x
pick_y = message.source.y
pick_x_threshold = message.source_x_tolerance
pick_y_threshold = message.source_y_tolerance
block_id = message.id
place_x = message.target.x
place_y = message.target.y
place_x_threshold = message.target_x_tolerance
place_y_threshold = message.target_y_tolerance
self.move_block(block_id, pick_x, pick_y, pick_x_threshold,
pick_y_threshold, place_x, place_y, place_x_threshold,
place_y_threshold, message.block_size)
def handle_pick_failure(self, exception):
rospy.loginfo("Pick failed, going home.")
self.open_gripper()
self.home_arm()
raise exception
def handle_place_failure(self, safe_zone, block_size, exception):
#should probably figure out if I'm holding the block first so it doesn't look weird
#figure out how to drop the block somewhere safe
#pass none and none if you are certain you haven't picked up a block yet
if safe_zone is None and block_size is None:
self.home_arm()
raise exception
rospy.loginfo("HANDLING PLACE FAILURE")
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(
safe_zone[0]['x'],
safe_zone[0]['y'],
safe_zone[1]['x_tolerance'],
safe_zone[1]['y_tolerance'],
current_block_state,
block_size,
num_attempts=1000)
try:
arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("panic arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
except Exception as e:
rospy.loginfo(
"ERROR: Cannot panic place the block! Get ready to catch it!")
self.open_gripper()
self.home_arm()
raise exception
def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
return candidate_blocks
def move_block(self,
block_id,
pick_x,
pick_y,
pick_x_tolerance,
pick_y_tolerance,
place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
block_size=None,
safe_zone=None,
pick_tries=2,
place_tries=1,
point_at_block=True):
if block_size is None:
block_size = get_ros_param('DEFAULT_BLOCK_SIZE')
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
# check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user
# moved while we are checking for a drop location
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
None, None,
Exception("no room in the target zone to drop the block"))
# here we are actually building a set of candidate blocks to pick
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
# select best block to pick and pick up
pick_location = None
if len(candidate_blocks) < 1:
raise Exception("no block of id " + str(block_id) +
" found within the source zone")
elif len(candidate_blocks) == 1:
pick_location = Point(candidate_blocks[0]['x'],
candidate_blocks[0]['y'], 0)
else:
pick_location = Point(*self.find_most_isolated_block(
candidate_blocks, current_block_state))
if point_at_block == True:
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.close_gripper()
self.point_at_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0))
self.point_at_block(location=Point(arm_drop_location[0],
arm_drop_location[1], 0))
self.home_arm()
except Exception as e:
rospy.loginfo(str(e))
rospy.loginfo("failed trying to point at block. giving up.")
self.home_arm()
self.move_block_server.set_succeeded(
MoveBlockResult(drop_location))
return
for i in range(pick_tries):
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.pick_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0),
check_grasp=True)
break
except Exception as e:
if i < pick_tries - 1:
rospy.loginfo("pick failed and trying again..." + str(e))
else:
rospy.loginfo("pick failed and out of attempts..." +
str(e))
self.handle_pick_failure(e)
if safe_zone == None:
if self.safe_zone == None:
safe_zone = [{
'x': pick_x,
'y': pick_y
}, {
'x_tolerance': pick_x_tolerance,
'y_tolerance': pick_y_tolerance
}]
else:
safe_zone = self.safe_zone
# calculate drop location
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
safe_zone, block_size,
Exception("no room in the target zone to drop the block"))
rospy.loginfo("tuio drop" + str(drop_location))
for i in range(place_tries):
try:
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
break
except Exception as e:
if i < place_tries - 1:
rospy.loginfo("place failed and trying again..." + str(e))
else:
rospy.loginfo("place failed and out of attempts..." +
str(e))
# check to see if we've defined a safe zone to drop the blocks
self.handle_place_failure(safe_zone, block_size, e)
# assume success if we made it this far
self.move_block_server.set_succeeded(MoveBlockResult(drop_location))
# check if a certain x, y position is within the bounds of another x,y position
@staticmethod
def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold):
if x <= x_origin + x_threshold and x >= x_origin - x_threshold \
and y <= y_origin + y_threshold and y >= y_origin - y_threshold:
return True
return False
# calculate the best location to drop the block
def calculate_drop_location(self,
x,
y,
x_threshold,
y_threshold,
blocks,
block_size,
num_attempts=1000):
attempt = 0
x_original, y_original = x, y
while (attempt < num_attempts):
valid = True
for block in blocks:
if self.check_block_bounds(block['x'], block['y'], x, y,
0.8 * block_size, block_size):
valid = False
break
if valid:
return Point(x, y, 0)
else:
x = random.uniform(x_original - x_threshold,
x_original + x_threshold)
y = random.uniform(y_original - y_threshold,
y_original + y_threshold)
attempt += 1
#if none found in num_attempts, return none
return None
# candidates should have more than one element in it
@staticmethod
def find_most_isolated_block(candidates, all_blocks):
print(candidates)
min_distances = [] # tuples of candidate, distance to closest block
for candidate in candidates:
min_dist = -1
for block in all_blocks:
if block['x'] == candidate['x'] and block['y'] == candidate[
'y']:
continue
else:
dist = DaArmServer.block_dist(candidate, block)
if min_dist == -1 or dist < min_dist:
min_dist = dist
min_distances.append([candidate, min_dist])
most_isolated, _ = max(min_distances, key=lambda x: x[
1]) # get most isolated candidate, and min_distance
return most_isolated['x'], most_isolated['y'], 0
@staticmethod
def block_dist(block_1, block_2):
return np.sqrt((block_2['x'] - block_1['x'])**2 +
(block_2['y'] - block_1['y'])**2)
def update_finger_position(self, message):
self.finger_positions = [
message.finger1, message.finger2, message.finger3
]
def check_grasp(self):
closed_pos = 0.95 * 6800
distance_from_closed = 0
for fp in self.finger_positions:
distance_from_closed += (closed_pos - fp)**2
if np.sqrt(distance_from_closed
) > 130: #this is just some magic number for now
return True #if the fingers aren't fully closed, then grasp is good
else:
return False
def open_gripper(self, delay=0):
"""open the gripper ALL THE WAY, then delay
"""
if self.is_simulation is True:
self.gripper.set_named_target("Open")
self.gripper.go()
else:
try:
rospy.loginfo("Opening Gripper!!")
self.move_fingers(50, 50, 50)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def close_gripper(self, delay=0):
"""close the gripper ALL THE WAY, then delay
"""
# self.gripper.set_named_target("Close")
# self.gripper.go()
try:
rospy.loginfo("Closing Gripper!!")
self.move_fingers(95, 95, 95)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def handle_gesture(self, gesture):
# lookup the gesture from a table? or how about a message?
# one possibility, object that contains desired deltas in pos/orientation
# as well as specify the arm or gripper choice
pass
def handle_move_pose(self, message):
# takes a geometry_msgs/Pose message
self.move_arm_to_pose(message.target.position,
message.target.orientation,
action_server=self.move_pose_server)
self.move_pose_server.set_succeeded()
def check_plan_result(self, target_pose, cur_pose):
#we'll do a very lenient check, this is to find failures, not error
#also only checking position, not orientation
rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose))
if np.abs(target_pose.pose.position.x -
cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2:
print("x error too far")
return False
if np.abs(target_pose.pose.position.y -
cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2:
print("y error too far")
return False
if np.abs(target_pose.pose.position.z -
cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2:
print("z error too far")
return False
print("tolerable error")
return True
# expects cooridinates for position to be in arm space
def move_arm_to_pose(self,
position,
orientation,
delay=0.5,
waypoints=[],
corrections=4,
action_server=None):
for i in range(corrections + 1):
rospy.loginfo("TRY NUMBER " + str(i))
if len(waypoints) > 0 and i < 1:
# this is good for doing gestures
plan, fraction = self.arm.compute_cartesian_path(
waypoints, eef_step=0.01, jump_threshold=0.0)
else:
p = self.arm.get_current_pose()
p.pose.position = position
p.pose.orientation = orientation
rospy.loginfo("PLANNING TO " + str(p))
self.arm.set_pose_target(p)
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
plan = self.arm.go(wait=False)
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 90 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 90 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
if (self.check_plan_result(p, self.arm.get_current_pose())):
break #we're there, no need to retry
#rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE")
if False:
# # get the last pose to correct if desired
# ptPos = plan.joint_trajectory.points[-1].positions
# # print "=================================="
# # print "Last point of the current trajectory: "
# angle_set = list()
# for i in range(len(ptPos)):
# tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
# angle_set.append(tempPos)
if action_server:
pass
#action_server.publish_feedback(CalibrateFeedback("Plan Found"))
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
self.arm.execute(plan, wait=False)
# this is a bit naive, but I'm going to loop until a new trajectory goal gets published
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 15 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 15 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
else:
if action_server:
#action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
pass
# Let's have the caller handle sequences instead.
# def handle_move_sequence(self, message):
# # if the move fails, do we cancel the sequence
# cancellable = message.cancellable
# moves = message.moves
# for move in moves:
# try:
# self.handle_move_block(move)
# except Exception:
# if cancellable:
# rospy.loginfo("Part of move failed, cancelling the rest.")
# break
def update_move_group_state(self, message):
rospy.loginfo(message.feedback.state)
self.move_group_state = message.feedback.state
def update_move_group_status(self, message):
if message.status_list:
#rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status))
self.move_group_status = message.status_list[0].status
def update_joint_trajectory_state(self, message):
# print(message.status_list)
if len(message.status_list) > 0:
self.joint_trajectory_state = message.status_list[0].status
else:
self.joint_trajectory_state = 0
def update_joint_trajectory_goal(self, message):
#print("goalisasis", message.goal_id.id)
self.last_joint_trajectory_goal = message.goal_id.id
def update_joint_trajectory_result(self, message):
#print("resultisasis", message.status.goal_id.id)
self.last_joint_trajectory_result = message.status.goal_id.id
def get_grasp_orientation(self, position):
#return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2))
return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633)
def update_joints(self, joints):
self.joint_angles = [
joints.joint1, joints.joint2, joints.joint3, joints.joint4,
joints.joint5, joints.joint6, joints.joint7
]
def move_z_relative(self, distance):
p = self.arm.get_current_pose()
p.pose.position.z += distance
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def move_z_absolute(self, height):
p = self.arm.get_current_pose()
p.pose.position.z = height
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def pick_block(self,
location,
check_grasp=False,
retry_attempts=0,
delay=0,
action_server=None):
# go to a spot and pick up a block
# if check_grasp is true, it will check torque on gripper and retry or fail if not holding
# open the gripper
# print('Position: ', position)
self.open_gripper()
position = Point(location.x, location.y, self.cruising_height)
orientation = self.get_grasp_orientation(position)
try:
self.raise_table()
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
position = Point(location.x, location.y, self.grasp_height)
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.crusing_height)
self.raise_table()
raise (
e
) #problem because sometimes we get exception e.g. if we're already there
# and it will skip the close if so.
if action_server:
action_server.publish_feedback()
self.close_gripper()
self.move_z_absolute(self.cruising_height)
#try to wait until we're up to check grasp
rospy.sleep(0.5)
if check_grasp is True:
if (self.check_grasp() is False):
print("Grasp failed!")
self.raise_table()
raise (Exception("grasp failed!"))
# for now, but we want to check finger torques
# for i in range(retry_attempts):
# self.move_z(0.3)
self.raise_table()
rospy.sleep(delay)
def place_block(self,
location,
check_grasp=False,
delay=0,
action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.drop_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.open_gripper()
self.move_z_absolute(self.cruising_height)
self.raise_table()
self.close_gripper()
def point_at_block(self, location, delay=0, action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.pointing_height)
self.move_z_absolute(self.cruising_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.raise_table()
def stop_motion(self, home=False, pause=False):
rospy.loginfo("STOPPING the ARM")
# cancel the moveit_trajectory
# self.arm.stop()
# call the emergency stop on kinova
self.emergency_stop()
# rospy.sleep(0.5)
if pause is True:
self.paused = True
if home is True:
# self.restart_arm()
self.home_arm()
def calibrate(self, message):
print("calibrating ", message)
self.place_calibration_block()
rospy.sleep(5) # five seconds to correct the drop if it bounced, etc.
print("moving on...")
self.calibration_server.publish_feedback(
CalibrateFeedback("getting the coordinates"))
params = self.record_calibration_params()
self.set_arm_calibration_params(params[0], params[1])
self.calibration_publisher.publish(
CalibrationParams(params[0], params[1]))
self.calibration_server.set_succeeded(CalibrateResult(params))
def set_arm_calibration_params(self, arm_x_min, arm_y_min):
rospy.set_param("ARM_X_MIN", arm_x_min)
rospy.set_param("ARM_Y_MIN", arm_y_min)
def place_calibration_block(self):
# start by homing the arm (kinova home)
self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
self.home_arm_kinova()
# go to grab a block from a human
self.open_gripper()
self.close_gripper()
self.open_gripper(4)
self.close_gripper(2)
# move to a predetermined spot
self.calibration_server.publish_feedback(
CalibrateFeedback("moving to drop"))
try:
self.move_arm_to_pose(Point(0.4, -0.4, 0.1),
Quaternion(1, 0, 0, 0),
corrections=0)
except Exception as e:
rospy.loginfo("THIS IS TH PRoblem" + str(e))
# drop the block
self.open_gripper()
self.calibration_server.publish_feedback(
CalibrateFeedback("dropped the block"))
calibration_block = {'x': 0.4, 'y': -0.4, 'id': 0}
calibration_action_belief = {
"action": "add",
"block": calibration_block
}
self.action_belief_publisher.publish(
String(json.dumps(calibration_action_belief)))
rospy.loginfo("published arm belief")
return
def record_calibration_params(self):
""" Call the block_tracker service to get the current table config.
Find the calibration block and set the appropriate params.
"""
# make sure we know the dimensions of the table before we start
# fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
tuio_bounds = get_tuio_bounds()
arm_bounds = get_arm_bounds(calibrate=False)
try:
block_state = json.loads(self.get_block_state("tuio").tui_state)
except rospy.ServiceException as e:
# self.calibration_server.set_aborted()
raise (ValueError("Failed getting block state to calibrate: " +
str(e)))
if len(block_state) != 1:
# self.calibration_server.set_aborted()
raise (ValueError(
"Failed calibration, either couldn't find block or > 1 block on TUI!"
))
# if we made it here, let's continue!
# start by comparing the reported tuio coords where we dropped the block
# with the arm's localization of the end-effector that dropped it
# (we assume that the block fell straight below the drop point)
drop_pose = self.arm.get_current_pose()
end_effector_x = drop_pose.pose.position.x
end_effector_y = drop_pose.pose.position.y
block_tuio_x = block_state[0]['x']
block_tuio_y = block_state[0]['y']
x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y,
tuio_bounds)
arm_x_min = end_effector_x - x_ratio * arm_bounds["x_dist"]
arm_y_min = end_effector_y - y_ratio * arm_bounds["y_dist"]
return [arm_x_min, arm_y_min]
class BaxterMoveit(BaxterRobot):
LEFT = -1
RIGHT = 1
def __init__(self, node, limb=+1):
super(BaxterMoveit, self).__init__(node, limb=+1)
self.group = MoveGroupCommander(self.lns + "_arm")
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self._info()
def _info(self):
'''Getting Basic Information'''
# name of the reference frame for this robot:
print("@@@@@@@@@@@@ Reference frame: %s" %
self.group.get_planning_frame())
# We can also print the name of the end-effector link for this group:
print("@@@@@@@@@@@@ End effector: %s" %
self.group.get_end_effector_link())
# We can get a list of all the groups in the robot:
print("@@@@@@@@@@@@ Robot Groups: @@@@@@@@@@@@@",
self.robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("@@@@@@@@@@@@ Printing robot state @@@@@@@@@@@@@")
print(self.robot.get_current_state())
def obstacle(self, name, position, size):
planning_frame = self.robot.get_planning_frame()
pose = PoseStamped()
pose.header.frame_id = planning_frame
pose.pose.position.x = position[0]
pose.pose.position.y = position[1]
pose.pose.position.z = position[2]
self.scene.add_box(name, pose, tuple(size))
def movej(self, q, raw=False):
''' move in joint space by giving a joint configuration as dictionary'''
if raw:
print("in moveit 'raw' motion is not avaiable")
# succ = False
# while succ is False:
succ = self.group.go(q, wait=True)
# self.group.stop() # ensures that there is no residual movement
def showplan(self, target):
if type(target) is PyKDL.Frame or type(target) is Pose:
q = self.ik(target)
elif type(target) is dict:
q = target
else:
print("Target format error")
return
self.group.set_joint_value_target(q)
self.group.plan()
def movep(self, pose, raw=False):
''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame'''
if type(pose) is PyKDL.Frame:
pose = transformations.KDLToPose(pose)
q = self.ik(pose)
if q is not None:
self.movej(q, raw=raw)
else:
print("\nNO SOLUTION TO IK\n" * 20)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置每次运动规划的时间限制：1s
arm.set_planning_time(1)
# 控制机械臂运动到之前设置的“home”姿态
arm.set_named_target('home')
arm.go()
# 获取当前位姿数据为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# --------------------- 第一段轨迹生成，关节空间插值 ---------------------#
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0, -1, 1, 0, -1, 0]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(0.5)
# 获取当前位姿数据为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# --------------------- 第一段轨迹生成，关节空间插值 ---------------------#
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.3米，向右运动0.3米
wpose = deepcopy(start_pose)
wpose.orientation.x = 0
wpose.orientation.y = 0.707106781186547
wpose.orientation.z = 0
wpose.orientation.w = 0.707106781186547
wpose.position.x -= 0.2
# wpose.position.y -= 0.4
wpose.position.z -= 1
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第三个路点数据，并加入路点列表
wpose.orientation.x = 0
wpose.orientation.y = 0.707106781186547
wpose.orientation.z = 0
wpose.orientation.w = 0.707106781186547
# wpose.position.x -= 0.5
wpose.position.y -= 0.5
wpose.position.z += 0.2
# wpose.position.x += 0.15
# wpose.position.y += 0.1
# wpose.position.z -= 0.15
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
roscpp_initialize(sys.argv)
rospy.init_node('pumpkin_planning', anonymous=True)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] = -0.357569385437786
group_variable_values[1] = -0.6320268016619928
group_variable_values[2] = 0.24177580736353846
group_variable_values[3] = 1.586101553004471
group_variable_values[4] = -0.5805943181752088
group_variable_values[5] = -1.1821499952996368
right_arm.set_joint_value_target(group_variable_values)
right_arm.go(wait=True)
print "============ Waiting while RVIZ displays Up..."
group_variable_values[0] = 1.0
right_arm.set_joint_value_target(group_variable_values)
right_arm.go(wait=True)
group_variable_values = right_arm.get_current_joint_values()
for i in xrange(6):
group_variable_values[i] = 0
right_arm.set_joint_value_target(group_variable_values)
class SrRobotCommander(object):
"""
Base class for hand and arm commanders
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy(
"get_robot_state", GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {
item["name"]: item["joints"]
for item in controller_list_param
}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def set_planner_id(self, planner_id):
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(
num_planning_attempts)
def set_planning_time(self, seconds):
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
"""
if self.check_plan_is_valid():
self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
def execute_plan(self, plan):
if self.check_given_plan_is_valid(plan):
self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
def move_to_joint_value_target(self,
joint_states,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not
This is a blocking method.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None
and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
self._move_group_commander.set_joint_value_target(js)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander.\
_g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return geometry_msgs.msg.Pose() - current pose of the end effector
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(
reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0), rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn(
"Couldn't get the pose from " +
self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return a dictionary with the joint names as keys and current joint values
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return a dictionary with the joint names as keys and current joint values
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position
@return - dictionary with joints positions
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = current.values()
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(
wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr(
"Invalid waypoint. Must contain valid name for named target or dict of joint angles."
)
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[
n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [
new_positions[n] for n in joint_names
]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = current.values()
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = current.keys()
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link=""):
"""
Specify a target position for the end-effector and plans.
This is a blocking method.
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self.__plan = self._move_group_commander.plan()
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self,
pose,
end_effector_link="",
alternative_method=False):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param alternative_method - use set_joint_value_target instead of set_pose_target
"""
self._move_group_commander.set_start_state_to_current_state()
if alternative_method:
self._move_group_commander.set_joint_value_target(
pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {
n: p
for n, p in zip(joint_state.name, joint_state.position)
}
self._joints_velocity = {
n: v
for n, v in zip(joint_state.name, joint_state.velocity)
}
self._joints_effort = {
n: v
for n, v in zip(joint_state.name, joint_state.effort)
}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(
service_name, FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(
timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self,
joint_states,
time=0.002,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller)
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client],
lambda terminal_state, result: self._action_done_cb(
client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i]
for i in indices_of_joints_in_this_controller
]
for point in goal.trajectory.points:
if point.positions:
point.positions = [
point.positions[i]
for i in indices_of_joints_in_this_controller
]
if point.velocities:
point.velocities = [
point.velocities[i]
for i in indices_of_joints_in_this_controller
]
if point.effort:
point.effort = [
point.effort[i]
for i in indices_of_joints_in_this_controller
]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self,
waypoints,
reference_frame=None,
eef_step=0.005,
jump_threshold=0.0):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given
@param waypoints - an array of poses of end-effector
@param eef_step - configurations are computed for every eef_step meters
@param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
"""
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
(self.__plan,
fraction) = self._move_group_commander.compute_cartesian_path(
waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory
@param trajectory - moveit_msgs/JointTrajectory
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
)
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.5
service_request.avoid_collisions = avoid_collisions
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" %
resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
arm.plan() and arm.go()
# rotate
for i in range(4):
# close
rospy.loginfo("close")
close_hand()
# rotate
angles = arm.get_current_joint_values()
import numpy
start_angle = angles[5]
print("current angles=", start_angle)
for r in numpy.arange(start_angle, start_angle-3.14*2, -1.0):
rospy.loginfo(angles)
angles[5] = r
rospy.loginfo("rotate (%f)" % (r))
msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
msg.trajectory.points = [JointTrajectoryPoint(positions=angles, time_from_start=rospy.Duration(1))]
client.send_goal(msg)
client.wait_for_result()
# open
rospy.loginfo("open")
open_hand()
# back
angles[5] = start_angle
arm.set_joint_value_target(angles)
rospy.loginfo("rotate (%f)" % (r))
arm.plan() and arm.go()
rospy.loginfo("done")
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "grasp" pose stored in the SRDF file
arm.set_named_target('left_arm_up')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.04
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.21
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
#scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.19
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
#scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.24
target_pose.pose.position.y = 0.275
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = 0
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
#grasp_pose.pose.position.y -= target_size[1] / 2.0
grasp_pose.pose.position.x = 0.12792118579 + .1
grasp_pose.pose.position.y = 0.285290879999 + 0.05
grasp_pose.pose.position.z = 0.120301181892
#grasp_pose.pose.orientation =
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id, table_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
break
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('left_arm_rest')
arm.go()
# Open the gripper to the open position
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 设置桌面的高度
table_ground = 0.2
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.31
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.29
box2_pose.pose.position.y = -0.4
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.04, 0.04, 0.05]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.32
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.32
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪回到张开的状态
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
rospy.init_node('arm_tracker')
rospy.on_shutdown(self.shutdown)
# Maximum distance of the target before the arm will lower
self.max_target_dist = 1.2
# Arm length to center of gripper frame
self.arm_length = 0.4
# Distance between the last target and the new target before we move the arm
self.last_target_threshold = 0.01
# Distance between target and end-effector before we move the arm
self.target_ee_threshold = 0.025
# Initialize the move group for the right arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set the reference frame for pose targets
self.reference_frame = REFERENCE_FRAME
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the right arm reference frame accordingly
self.right_arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.right_arm.allow_replanning(False)
# Set a position tolerance in meters
self.right_arm.set_goal_position_tolerance(0.05)
# Set an orientation tolerance in radians
self.right_arm.set_goal_orientation_tolerance(0.2)
# What is the end effector link?
self.ee_link = self.right_arm.get_end_effector_link()
# Create the transform listener
self.listener = tf.TransformListener()
# Queue up some tf data...
rospy.sleep(3)
# Set the gripper target to closed position using a joint value target
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute the gripper motion
right_gripper.go()
rospy.sleep(1)
# Subscribe to the target topic
rospy.wait_for_message('/target_pose', PoseStamped)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
rospy.loginfo("Ready for action!")
while not rospy.is_shutdown():
try:
target = self.target
except:
rospy.sleep(0.5)
continue
# Timestamp the target with the current time
target.header.stamp = rospy.Time()
# Get the target pose in the right_arm shoulder lift frame
#target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
target_arm = self.listener.transformPose('right_arm_base_link', target)
# Convert the position values to a Python list
p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z]
# Compute the distance between the target and the shoulder link
dist_target_shoulder = euclidean(p0, [0, 0, 0])
# If the target is too far away, then lower the arm
if dist_target_shoulder > self.max_target_dist:
rospy.loginfo("Target is too far away")
self.right_arm.set_named_target('resting')
self.right_arm.go()
rospy.sleep(1)
continue
# Transform the pose to the base reference frame
target_base = self.listener.transformPose(self.reference_frame, target)
# Compute the distance between the current target and the last target
p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
dist_last_target = euclidean(p1, p2)
# Move the arm only if we are far enough away from the previous target
if dist_last_target < self.last_target_threshold:
rospy.loginfo("Still close to last target")
rospy.sleep(0.5)
continue
# Get the pose of the end effector in the base reference frame
ee_pose = self.right_arm.get_current_pose(self.ee_link)
# Convert the position values to a Python list
p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z]
# Compute the distance between the target and the end-effector
dist_target = euclidean(p1, p3)
# Only move the arm if we are far enough away from the target
if dist_target < self.target_ee_threshold:
rospy.loginfo("Already close enough to target")
rospy.sleep(1)
continue
# We want the gripper somewhere on the line connecting the shoulder and the target.
# Using a parametric form of the line, the parameter ranges from 0 to the
# minimum of the arm length and the distance to the target.
t_max = min(self.arm_length, dist_target_shoulder)
# Bring it back 10% so we don't collide with the target
t = 0.9 * t_max
# Now compute the target positions from the parameter
try:
target_arm.pose.position.x *= (t / dist_target_shoulder)
target_arm.pose.position.y *= (t / dist_target_shoulder)
target_arm.pose.position.z *= (t / dist_target_shoulder)
except:
rospy.sleep(1)
rospy.loginfo("Exception!")
continue
# Transform to the base_footprint frame
target_ee = self.listener.transformPose(self.reference_frame, target_arm)
# Set the target gripper orientation to be horizontal
target_ee.pose.orientation.x = 0
target_ee.pose.orientation.y = 0
target_ee.pose.orientation.z = 0
target_ee.pose.orientation.w = 1
# Update the current start state
self.right_arm.set_start_state_to_current_state()
# Set the target pose for the end-effector
self.right_arm.set_pose_target(target_ee, self.ee_link)
# Plan and execute the trajectory
success = self.right_arm.go()
if success:
# Store the current target as the last target
self.last_target_pose = target
# Pause a bit between motions to keep from locking up
rospy.sleep(0.5)
def __init__(self):
rospy.init_node('arm_tracker')
rospy.on_shutdown(self.shutdown)
# Maximum distance of the target before the arm will lower
self.max_target_dist = 1.2
# Arm length to center of gripper frame
self.arm_length = 0.4
# Distance between the last target and the new target before we move the arm
self.last_target_threshold = 0.01
# Distance between target and end-effector before we move the arm
self.target_ee_threshold = 0.025
# Initialize the move group for the left arm
self.left_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the left gripper
left_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set the reference frame for pose targets
self.reference_frame = REFERENCE_FRAME
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the left arm reference frame accordingly
self.left_arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.left_arm.allow_replanning(False)
# Set a position tolerance in meters
self.left_arm.set_goal_position_tolerance(0.05)
# Set an orientation tolerance in radians
self.left_arm.set_goal_orientation_tolerance(0.2)
# What is the end effector link?
self.ee_link = self.left_arm.get_end_effector_link()
# Create the transform listener
self.listener = tf.TransformListener()
# Queue up some tf data...
rospy.sleep(3)
# Set the gripper target to closed position using a joint value target
left_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute the gripper motion
left_gripper.go()
rospy.sleep(1)
# Subscribe to the target topic
rospy.wait_for_message('/target_pose', PoseStamped)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('/target_pose',
PoseStamped,
self.update_target_pose,
queue_size=1)
rospy.loginfo("Ready for action!")
while not rospy.is_shutdown():
try:
target = self.target
except:
rospy.sleep(0.5)
continue
# Timestamp the target with the current time
target.header.stamp = rospy.Time()
# Get the target pose in the left_arm shoulder lift frame
#target_arm = self.listener.transformPose('left_arm_shoulder_pan_link', target)
target_arm = self.listener.transformPose('left_rotate', target)
# Convert the position values to a Python list
p0 = [
target_arm.pose.position.x, target_arm.pose.position.y,
target_arm.pose.position.z
]
# Compute the distance between the target and the shoulder link
dist_target_shoulder = euclidean(p0, [0, 0, 0])
# If the target is too far away, then lower the arm
if dist_target_shoulder > self.max_target_dist:
rospy.loginfo("Target is too far away")
self.left_arm.set_named_target('l_start')
self.left_arm.go()
rospy.sleep(1)
continue
# Transform the pose to the base reference frame
target_base = self.listener.transformPose(self.reference_frame,
target)
# Compute the distance between the current target and the last target
p1 = [
target_base.pose.position.x, target_base.pose.position.y,
target_base.pose.position.z
]
p2 = [
self.last_target_pose.pose.position.x,
self.last_target_pose.pose.position.y,
self.last_target_pose.pose.position.z
]
dist_last_target = euclidean(p1, p2)
# Move the arm only if we are far enough away from the previous target
if dist_last_target < self.last_target_threshold:
rospy.loginfo("Still close to last target")
rospy.sleep(0.5)
continue
# Get the pose of the end effector in the base reference frame
ee_pose = self.left_arm.get_current_pose(self.ee_link)
# Convert the position values to a Python list
p3 = [
ee_pose.pose.position.x, ee_pose.pose.position.y,
ee_pose.pose.position.z
]
# Compute the distance between the target and the end-effector
dist_target = euclidean(p1, p3)
# Only move the arm if we are far enough away from the target
if dist_target < self.target_ee_threshold:
rospy.loginfo("Already close enough to target")
rospy.sleep(1)
continue
# We want the gripper somewhere on the line connecting the shoulder and the target.
# Using a parametric form of the line, the parameter ranges from 0 to the
# minimum of the arm length and the distance to the target.
t_max = min(self.arm_length, dist_target_shoulder)
# Bring it back 10% so we don't collide with the target
t = 0.9 * t_max
# Now compute the target positions from the parameter
try:
target_arm.pose.position.x *= (t / dist_target_shoulder)
target_arm.pose.position.y *= (t / dist_target_shoulder)
target_arm.pose.position.z *= (t / dist_target_shoulder)
except:
rospy.sleep(1)
rospy.loginfo("Exception!")
continue
# Transform to the base_footprint frame
target_ee = self.listener.transformPose(self.reference_frame,
target_arm)
# Set the target gripper orientation to be horizontal
target_ee.pose.orientation.x = 0
target_ee.pose.orientation.y = 0
target_ee.pose.orientation.z = 0
target_ee.pose.orientation.w = 1
# Update the current start state
self.left_arm.set_start_state_to_current_state()
# Set the target pose for the end-effector
self.left_arm.set_pose_target(target_ee, self.ee_link)
# Plan and execute the trajectory
success = self.left_arm.go()
if success:
# Store the current target as the last target
self.last_target_pose = target
# Pause a bit between motions to keep from locking up
rospy.sleep(0.5)
joint_target_4, joint_target_5, joint_target_init
]
joint_targets = []
for i in range(0, 361, 45):
for j in range(-20, 21, 20):
i_rads = i * math.pi / 180
j_rads = j * math.pi / 180
joint_target = rotate_joint(joint_target_init, i_rads,
JOINT_INDEX_BASE)
joint_target = rotate_joint(joint_target, j_rads,
JOINT_INDEX_UPPER_ARM)
joint_targets.append(joint_target)
for joint_target in joint_targets:
group.set_joint_value_target(joint_target)
group.set_max_velocity_scaling_factor(0.1)
plan = group.plan()
group.go(wait=True)
group.stop()
if rospy.is_shutdown():
break
rospy.sleep(1)
roscpp_shutdown()
