def gotoInit(self):
move_group = MoveGroupInterface("arm", "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
# Plans the joints in joint_names to angles in pose
init = self.trajActionGoal.trajectory.points[0].positions
move_group.moveToJointPosition(joint_names, init, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Moved to Init")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
class GripperClient:
def __init__(self):
self.move_group = MoveGroupInterface("gripper", "base_link")
self.joint_names = ['l_gripper_finger_joint', 'r_gripper_finger_joint']
def open_gripper(self):
value = [0.1, 0.1]
self.__execu(value)
# Plans the joints in joint_names to angles in pose
def close_gripper(self):
value = [0.0, 0.0]
self.__execu(value)
# Plans the joints in joint_names to angles in pose
def __execu(self, value):
self.move_group.moveToJointPosition(self.joint_names,
value,
wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print 'gripper'
else:
# If you get to this point please search for:
rospy.logerr(
"Arm goal in state: %s",
self.move_group.get_move_action().get_joint_values())
else:
rospy.logerr("MoveIt! failure no result returned.")
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface(
"manipulator", "base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = [
"shoulder_pan_joint",
"shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
]
# pose = [-1.26 , -0.64 , -2.44 , -0.66 , 1.56 , 0.007]
# move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす
# move_group.get_move_action().wait_for_result() #wait result
# result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
self.state = {
'default': 0,
'hold': 1,
'way_standby': 2,
'plan_standby': 3
}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
self.now_state = self.state['default'] #現在フレームの状態
class TuckThread(Thread):
def __init__(self, untuck=False):
Thread.__init__(self)
self.client = None
self.untuck = untuck
self.start()
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
if not self.untuck:
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
else:
pose = [0.05, 0.29, -0.60, -0.51, 1.45, 0.52, 0.88, -1.97]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
def stop(self):
if self.client:
self.client.get_move_action().cancel_goal()
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("failed")
sys.exit(0)
def main(args):
rospy.init_node('simple_move')
move_group = MoveGroupInterface('manipulator', "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
# This is the wrist link not the gripper itself
gripper_frame = "wrist_3_link"
poses, pose_name = get_pose(args.reset)
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = "base_link"
while not rospy.is_shutdown():
for pose in poses:
# Finish building the Pose_stamped message
# If the message stampe is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
log_output = pose_name + " Done!"
rospy.loginfo(log_output)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす
move_group.get_move_action().wait_for_result() #wait result
result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
class Arm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6)
def MoveToPose(self, X, Y, Z, Roll, Pitch, Yaw):
orientation = tf.transformations.quaternion_from_euler(
Roll, Pitch, Yaw)
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = orientation[0]
self.poseStamped.pose.orientation.y = orientation[1]
self.poseStamped.pose.orientation.z = orientation[2]
self.poseStamped.pose.orientation.w = orientation[3]
self.moveGroup.moveToPose(self.poseStamped,
'gripper_link',
0.005,
max_velocity_scaling_factor=0.2)
self.result = self.moveGroup.get_move_action().get_result()
def Tuck(self):
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
self.moveGroup.moveToJointPosition(joints,
pose,
0.01,
max_velocity_scaling_factor=0.2)
while not rospy.is_shutdown():
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
def AddDice(self, name, X, Y, Z):
self.planning_scene.addCube(name, 0.1, X, Y, Z)
def RemoveDice(self, name):
self.planning_scene.removeCollisionObject(name)
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
class TuckThread(Thread):
def __init__(self):
Thread.__init__(self)
self.client = None
self.start()
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
def stop(self):
if self.client:
self.client.get_move_action().cancel_goal()
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("failed")
sys.exit(0)
class RobotArm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.removeCollisionObject("table1")
def MoveToPose(self,X,Y,Z,x,y,z,w):
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = x
self.poseStamped.pose.orientation.y = y
self.poseStamped.pose.orientation.z = z
self.poseStamped.pose.orientation.w = w
self.moveGroup.moveToPose(self.poseStamped, 'gripper_link')
self.result = self.moveGroup.get_move_action().get_result()
def OriginReturn(self):
#Move to the origin
joints = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
self.moveGroup.moveToJointPosition(joints, pose, 0.02)
while 1:
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
def arm_move(self):
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
#planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_left_ground", 1, 1.5,
self.translation[2][0], self.translation[2][1],
self.translation[2][2])
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Disco!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
class ArmController(object):
def __init__(self):
# Create move group interface for a fetch robot
self._move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
self._planning_scene = PlanningSceneInterface("base_link")
self._planning_scene.removeCollisionObject("my_front_ground")
self._planning_scene.removeCollisionObject("my_back_ground")
self._planning_scene.removeCollisionObject("my_right_ground")
self._planning_scene.removeCollisionObject("my_left_ground")
self._planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self._planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self._planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self._planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def SetPose(self, target_pose):
# Plans the joints in joint_names to angles in pose
self._move_group.moveToJointPosition(target_pose.keys(),
target_pose.values(),
wait=False)
# Since we passed in wait=False above we need to wait here
self._move_group.get_move_action().wait_for_result()
result = self._move_group.get_move_action().get_result()
return_result = False
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("MoveIt pose reached.")
return_result = True
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self._move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
self._move_group.get_move_action().cancel_all_goals()
return return_result
def SetPoseWithRetry(self, target_pose, max_time=3):
for _ in range(max_time):
if self.SetPose(target_pose):
return True
return False
def main(args):
rospy.init_node('simple_move')
move_group = MoveGroupInterface('manipulator', "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
poses, pose_name = get_pose(args.case, args.reset)
for pose in poses:
if rospy.is_shutdown():
break
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
log_output = pose_name + " Done!"
rospy.loginfo(log_output)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
class PoseController(object):
def __init__(self):
# Create move group interface for a fetch robot
self.move_group_ = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
self.planning_scene_ = PlanningSceneInterface("base_link")
self.planning_scene_.removeCollisionObject("my_front_ground")
self.planning_scene_.removeCollisionObject("my_back_ground")
self.planning_scene_.removeCollisionObject("my_right_ground")
self.planning_scene_.removeCollisionObject("my_left_ground")
self.planning_scene_.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene_.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene_.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene_.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.angle = np.zeros(FILTER_LENGTH)
self.last_trigger_time = rospy.Time.now()
rospy.Subscriber(AUDIO_TOPIC, SoundSourceAngle, self.SetPoseWithRetry)
rospy.spin()
def SetPoseWithRetry(self, msg, max_time=3):
if msg.is_valid:
direction = 0
(direction, self.angle) = TemporalMedian(msg.angle, self.angle)
soulder_pan = round((direction / 180.0) * np.pi, 2)
target_pose = {
'torso_lift_joint': 0.05287206172943115,
'shoulder_pan_joint': soulder_pan,
'shoulder_lift_joint': 0.0,
'upperarm_roll_joint': -np.pi,
'elbow_flex_joint': 0.0,
'forearm_roll_joint': np.pi,
'wrist_flex_joint': -1.5,
'wrist_roll_joint': 0.0,
}
if rospy.Time.now() - self.last_trigger_time > rospy.Duration(2):
rospy.loginfo("Shoulder pan")
rospy.loginfo(soulder_pan)
for _ in range(max_time):
if self.SetPose(target_pose):
return True
return False
def SetPose(self, target_pose):
# Plans the joints in joint_names to angles in pose
self.move_group_.moveToJointPosition(target_pose.keys(),
target_pose.values(),
wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group_.get_move_action().wait_for_result()
result = self.move_group_.get_move_action().get_result()
return_result = False
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Done!")
return_result = True
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group_.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
self.move_group_.get_move_action().cancel_all_goals()
self.last_trigger_time = rospy.Time.now()
return return_result
class FetchPose:
def __init__(self):
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso")
self.joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# planning_scene = PlanningSceneInterface("base_link")
# planning_scene.removeCollisionObject("my_front_ground")
# planning_scene.removeCollisionObject("my_back_ground")
# planning_scene.removeCollisionObject("my_right_ground")
# planning_scene.removeCollisionObject("my_left_ground")
# planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
# planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
# planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
# planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.tuck_the_arm_joints_value = [
0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0
]
self.stow_joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
# self.high_pose_value = [0.38380688428878784, -0.12386894226074219, -0.31408262252807617, 2.1759514808654785, 0.0061359405517578125, 0.9556700587272644, -1.921694278717041, -1.6908303499221802]
self.high_pose_value = [
0.4946120488643647, -0.19136428833007812, -0.4793691635131836,
0.009587380103766918, 0.1629854440689087, 0.2983585298061371,
1.8430781364440918, -1.6992675065994263
]
self.trans = Transformer()
def excute(self, joints_value):
if rospy.is_shutdown():
return False
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names,
joints_value,
wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("pose Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
def tuck_the_arm(self):
self.excute(self.tuck_the_arm_joints_value)
def stow(self):
self.excute(self.stow_joints_value)
def hige_pose(self):
self.excute(self.high_pose_value)
def control_torso(self, offset=0.1):
joints_values = self.pose_group.get_current_joint_values()
joints_values[0] += offset
self.excute(joints_values)
def grasp_by_move_group(self, point_xyz):
wrist_roll_link_gripper_offset = 0.20
q1 = self.trans.quaternion_from_direction((1, 0, 0), (0, 0, -1))
q2 = self.trans.quaternion_from_axis(90, (1, 0, 0))
quaternion = quaternion_multiply(q1, q2)
pose = Pose(
Point(point_xyz[0], point_xyz[1],
point_xyz[2] + wrist_roll_link_gripper_offset), # offset
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = pose
gripper_frame = 'wrist_roll_link'
self.move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = self.move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print("reach success!")
return 'Success'
else:
rospy.logerr("reach fail")
return 'Fail'
else:
rospy.logerr("MoveIt! failure no result returned.")
return 'Fail'
class GraspingClient(object):
def __init__(self):
self.arm = MoveGroupInterface('arm_with_torso', 'map')
self.move_group = MoveGroupInterface('arm', 'base_link')
def pick(self, target_pose):
#print self.arm._fixed_frame, self.arm.planner_id
#Taking coordinate values of the object
object_x, object_y, object_z = round(
target_pose.pose.position.x,
2), round(target_pose.pose.position.y,
2), round(target_pose.pose.position.z, 2)
print object_x, object_y, object_z
#Initializing offset values for translation
offset_x, offset_y, offset_z = 0, 0, 0.05
#Initializing offset values for rotation
roll, pitch, yaw = 0, 0, 0
#Applying rotation to the arm
for i in range(10, 1, -1):
#Adding translation along x-axis
final_x = object_x + offset_x
#Adding translation along y-axis
final_y = object_y + offset_y
#Adding translation along z-axis
final_z = object_z + offset_z + i * 0.1
print final_x, final_y, final_z, i * 0.1
final_quaternion = quaternion_from_euler(radians(roll),
radians(pitch),
radians(yaw), 'sxyz')
pose_goal = PoseStamped()
pose_goal.header.frame_id = 'map'
pose_goal.pose.position.x = final_x
pose_goal.pose.position.y = final_y
pose_goal.pose.position.z = final_z
pose_goal.pose.orientation.x = final_quaternion[0]
pose_goal.pose.orientation.y = final_quaternion[1]
pose_goal.pose.orientation.z = final_quaternion[2]
pose_goal.pose.orientation.w = final_quaternion[3]
pose_goal.header.stamp = rospy.Time.now()
self.arm.moveToPose(pose_goal, 'wrist_roll_link')
result = self.arm.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo('Moved arm')
rospy.sleep(10)
else:
rospy.logerr('Cannot move arm')
else:
rospy.logerr('No result')
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo('Tucking done')
def main():
rospy.init_node('move_group_python_path_follow', anonymous=True)
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
moveit_commander.roscpp_initialize(sys.argv)
move_group = MoveGroupInterface("arm", "base_link")
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
print "============"
gripper_frame = 'wrist_roll_link'
gripper_poses = list()
radius = 0.2
for theta in np.arange(-pi, pi, 0.5 * pi):
gripper_poses.append(
Pose(
Point(0.75, radius * np.sin(theta),
0.6 + radius * np.cos(theta)),
Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))))
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = gripper_poses[0]
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
trajectory = RobotTrajectory()
(plan, fraction) = group.compute_cartesian_path(gripper_poses, 0.01, 0.0)
group.execute(plan)
move_group.get_move_action().cancel_all_goals()
[0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0],
[0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
[0.385, -1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0],
[0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
[0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0],
[0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Disco!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
class TuckThread(Thread):
def __init__(self):
Thread.__init__(self)
self.client = None
self.start()
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
else:
rospy.loginfo("moveit already started")
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
keepout_pose = Pose()
keepout_pose.position.z = 0.375
keepout_pose.orientation.w = 1.0
ground_pose = Pose()
ground_pose.position.z = -0.03
ground_pose.orientation.w = 1.0
rospack = rospkg.RosPack()
mesh_dir = os.path.join(rospack.get_path('fetch_teleop'), 'mesh')
scene.addMesh('keepout', keepout_pose,
os.path.join(mesh_dir, 'keepout.stl'))
scene.addMesh('ground', ground_pose,
os.path.join(mesh_dir, 'ground.stl'))
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(
joints, pose, 0.0, max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
scene.removeCollisionObject("ground")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
def stop(self):
if self.client:
self.client.get_move_action().cancel_goal()
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("failed")
sys.exit(0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = ['l_gripper_finger_joint', 'r_gripper_finger_joint']
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0.01, 0.01]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
pass
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_joint_values())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
Q = [0, 0, 0, 0.0, -np.pi / 2, 0.0, np.pi / 2, 0.0]
move_group.moveToJointPosition(joint_names, Q, wait=False)
move_group.get_move_action().wait_for_result()
# position:
# x: 0.640036865808
# y: -0.0391988119345
# z: 1.11652027628
# orientation:
# x: 0.00118531019309
# y: 0.00278754644384
# z: -0.0268161732665
# w: 0.999635792414
L = np.array([[-0.086875, 0, 0.37743], [0.119525, 0, 0.34858],
[0.117, 0, 0.06], [0.219, 0, 0], [0.133, 0,
0], [0.197, 0, 0],
[0.1245, 0, 0], [0.1385, 0, 0], [0.16645, 0, 0]])
from geometry_msgs.msg import Pose, Point, Quaternion
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = '/base_link'
pose.pose = Pose(Point(0.042, 0.384, 0.826), Quaternion(0, 0, 0, 1))
world_manager.world_manager_client.add_mesh(
'spam', '/home/vlad/Downloads/spam_12oz.dae', pose)
### Using MoveIt to move arm -- this works by itsels, without using CURPP. The arm moves to this location.
import rospy
from moveit_msgs.msg import MoveItErrorCodes
from moveit_python import MoveGroupInterface, PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
rospy.init_node("hi")
move_group_arm = MoveGroupInterface("arm_with_torso", "base_link")
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'spam'
gripper_pose_stamped.pose = graspit_grasps.grasps[2].pose
# Move gripper frame to the pose specified
move_group_arm.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group_arm.get_move_action().get_result()
move_group_gripper = MoveGroupInterface("gripper_custom", "gripper_link")
gripper_frame = 'wrist_roll_link'
Lwrist2gripper = 0.12
X_pg = [X[0] - Lwrist2gripper - 0.20, X[1], X[2]]
X_g = [X[0] - Lwrist2gripper, X[1], X[2]]
X_up = [X_g[0], X_g[1], X_g[2] + 0.2]
print(X_pg)
print(X_g)
# Open gripper
move_group_gripper.moveToJointPosition(
['l_gripper_finger_joint', 'l_gripper_finger_joint'], [0.040, 0.040],
wait=False)
move_group_gripper.get_move_action().wait_for_result()
result = move_group_gripper.get_move_action().get_result()
gripper_pose_pregrasp = create_pose_stamped_msg(X_pg, Q, 'base_link')
gripper_pose_grasp = create_pose_stamped_msg(X_g, Q, 'base_link')
gripper_pose_up = create_pose_stamped_msg(X_up, Q, 'base_link')
# # Create pre grasp pose msg
# gripper_pose_pregrasp = PoseStamped()
# gripper_pose_pregrasp.header.frame_id = "base_link"
# # do position and orientation seperately
# gripper_pose_pregrasp.pose.position = object_pose.g_pos.position
# gripper_pose_pregrasp.pose.orientation = object_pose.g_pos.orientation
# gripper_pose_pregrasp.pose.position.x = g_pos_x - Lwrist2gripper - 0.20
try:
t = listener.getLatestCommonTime(frame, "/map")
trans, rot = listener.lookupTransform(frame, "/map", t)
pos.append(trans)
ori.append(rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
break
if len(pos) == 8:
pos = np.array(pos)
acc = MotionPolicy(oldStates, pos, obstacle, goal)
oldStates = pos
acc = np.multiply(acc, ((rospy.get_time() - time)**2) / 2.0)
time = rospy.get_time()
poses = np.add(pos, acc)
pose = poses[-2]
rot = ori[-2]
gripper_pose = Pose(Point(pose[0], pose[1], pose[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = "/map"
gripper_pose_stamped.header.stamp = t
gripper_pose_stamped.pose = gripper_pose
move_group.moveToPose(gripper_pose_stamped, frames[-2][1:])
rate.sleep()
move_group.get_move_action().cancel_all_goals()
class FetchClient(object):
def __init__(self):
self.bfp = True
self.cx = 320
self.cy = 240
self.ax = 57.5
self.ay = 45
self.fx = self.cx / math.tan(self.ax * math.pi / 180.0 / 2)
self.fy = self.cy / math.tan(self.ay * math.pi / 180.0 / 2)
self.robot = robot_interface.Robot_Interface()
self.br = tf.TransformBroadcaster()
self.move_group = MoveGroupInterface("arm", "base_link")
self.pose_group = moveit_commander.MoveGroupCommander("arm")
self.cam = camera.RGBD()
while True:
try:
cam_info = self.cam.read_info_data()
if cam_info is not None:
break
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.pcm.fromCameraInfo(cam_info)
def broadcast_position(self, position, name):
pass
def calc_3D_position(self, u, v):
dep_data = self.robot.get_img_data()[1]
z = self.robot.get_depth((u, v), dep_data)
if z == 0:
return None, None, None
x = (u - self.cx) * z / self.fx
y = (v - self.cy) * z / self.fy
return x, y, z
def broadcast_from_pix(self, point, name):
while self.bfp:
# dep = self.robot.get_depth(point, dep_data)
# while dep == 0:
# print("depth info error")
# dep = self.robot.get_depth(point, dep_data)
#
# td_points = self.pcm.projectPixelTo3dRay(point)
# norm_pose = np.array(td_points)
# norm_pose = norm_pose / norm_pose[2]
# norm_pose = norm_pose * (dep)
norm_pose = self.calc_3D_position(point[0], point[1])
a = tf.transformations.quaternion_from_euler(ai=-2.355,
aj=-3.14,
ak=0.0)
b = tf.transformations.quaternion_from_euler(ai=0.0,
aj=0.0,
ak=1.57)
base_rot = tf.transformations.quaternion_multiply(a, b)
self.br.sendTransform(norm_pose, base_rot, rospy.Time.now(), name,
'head_camera_rgb_optical_frame')
def get_position_of_baselink(self, name):
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
pose, rot = listener.lookupTransform('base_link', name,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
rate.sleep()
if pose:
# rot = tf.transformations.quaternion_from_euler(0*DEG_TO_RAD, 90*DEG_TO_RAD, 0*DEG_TO_RAD)
return pose
def create_tf_of_base_link(self, name, position, rot):
quat = tf.transformations.quaternion_from_euler(ai=rot[0],
aj=rot[1],
ak=rot[2])
while True:
self.br.sendTransform(position, quat, rospy.Time.now(), name,
'base_link')
def show_image_with_point(self, im):
plt.imshow(im)
pylab.show()
def reach(self, point_xyz):
pose = Pose(Point(point_xyz[0], point_xyz[1], point_xyz[2]),
Quaternion(0, 0.707106781187, 0, 0.707106781187))
# pose = Pose(Point(0.57503179279, 0.170263536187, 0.97855338914), Quaternion(0, 0.707106781187, 0, 0.707106781187))
print(pose)
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = pose
gripper_frame = 'wrist_roll_link'
# gripper_frame = 'l_gripper_finger_link'
self.move_group.moveToPose(gripper_pose_stamped,
gripper_frame,
plan_only=True)
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
print("plan0 success!")
height = result.planned_trajectory.joint_trajectory.points[0]
gripper_pose_stamped.pose.position.z += height
self.move_group.moveToPose(gripper_pose_stamped,
gripper_frame,
plan_only=True)
result1 = self.move_group.get_move_action().get_result()
if result1:
if result1.error_code.val == MoveItErrorCodes.SUCCESS:
print("plan1 success!")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint",
"forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
joint_pose = result1.planned_trajectory.joint_trajectory[
0].position[6:13]
print joint_pose
self.move_group.moveToJointPosition(joint_names,
joint_pose,
wait=False)
result2 = self.move_group.get_move_action().get_result()
if result2:
# Checking the MoveItErrorCode
if result2.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("reach!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("reach fail")
else:
rospy.logerr("MoveIt! failure no result returned.")
else:
rospy.logerr("plan1 fail")
else:
rospy.logerr("MoveIt! failure no result returned.")
else:
rospy.logerr("MoveIt! failure no result returned.")
def move():
rospy.init_node("move")
#
#target pose
#
robot = moveit_commander.RobotCommander()
print "group name ",robot.get_group_names()
#print "current state ",robot.get_current_state()
group = moveit_commander.MoveGroupCommander("manipulator")
group_initial_pose = group.get_current_pose().pose
print "group initaial pose ",group_initial_pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = 0.0
pose_target.orientation.y = 0.0
pose_target.orientation.z = 0.0
pose_target.orientation.w = 1.0
pose_target.position.x = 0.5
pose_target.position.y = 0.5
pose_target.position.z = 0.9
group.set_pose_target(pose_target)
group.go()
#
#joint pose
#
move_group = MoveGroupInterface("manipulator","base_link")
#planning_scene = PlanningSceneInterface("base_link")
#setting up to initial robot state
# TF joint names
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
# Lists of joint angles in the same order as in joint_names
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
#
#cartesian path
#
#moveing along with catesian paths
group = moveit_commander.MoveGroupCommander("manipulator")
waypoints = []
# start with the current pose
waypoints.append(group.get_current_pose().pose)
# first orient gripper and move
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z - 0.05
waypoints.append(copy.deepcopy(wpose))
# second move
wpose.position.x += 0.03
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = group.compute_cartesian_path(waypoints,0.01,0.0) #waypoints to follow, eef_setup, jump_threshold
group.execute(plan)
class Node:
def __init__(self, image_topic, camera_info_topic, camera_frame,
published_point_num_topic, published_point_base_topic,
torso_movement_topic, head_movement_topic,
num_published_points, max_spine_height, min_spine_height,
spine_offset):
'''
Description: establishes the variabes included in the class
Input: self <Object>, image_topic <Object>, camera_info_topic <String>,
camera_frame <String>, published_point_num_topic <String>,
published_point_base_topic <String>, torso_movement_topic <String>,
head_movement_topic <String>, num_published_points <Int>,
max_spine_height <Int>, min_spine_height <Int>, spine_offset <Int>
Return: None
'''
self.camera_frame = camera_frame
self.published_point_base_topic = published_point_base_topic
self.bridge = CvBridge()
self.tf = TransformListener()
rospy.Subscriber(image_topic, Image, self.image_callback)
rospy.Subscriber(camera_info_topic, CameraInfo,
self.camera_info_callback)
self.robot_pose = None
self.img = None
self.pc = None
self.camera_info = None
self.num_published_points = num_published_points
self.published_points = [[0, 0, 0]
for i in range(num_published_points)]
for i in range(num_published_points):
rospy.Subscriber(self.published_point_base_topic + str(i),
PointStamped, self.point_published_callback, i)
self.points_registered = sets.Set()
# base movement
self.base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
self.base_client.wait_for_server()
# torso movement
self.max_spine_height = max_spine_height
self.min_spine_height = min_spine_height
self.spine_offset = spine_offset
# head movement
self.point_head_client = actionlib.SimpleActionClient(
head_movement_topic, PointHeadAction)
self.point_head_client.wait_for_server()
# keyboard listener
self.keypress_sub = rospy.Subscriber('/key_monitor', String,
self.key_callback)
rospy.loginfo("move group")
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("move group end")
def robot_pose_callback(self, data):
'''
Description: reset the ROS node pose to the stored value in data
Input: self <Object>, ar_tag_frame <String>, point topic <Object>
Return: None
'''
self.robot_pose = data
def image_callback(self, data):
'''
Description: reset the ROS node img data to the stored value in data
Input: self <Object>, data <Object>
Return: None
'''
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
def move_base_to(self, x, y, theta):
'''
Description: sends a position goal to the ROS node to move the robot
base to
Input: self <Object>, x <Int>, y <Int>, theta <Float>
Return: None
'''
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
quat = transformations.quaternion_from_euler(0, 0, theta)
orientation = Quaternion()
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
goal.target_pose.pose.orientation = orientation
# send goal
self.base_client.send_goal(goal)
def move_torso(self, pose):
'''
Description: calls the ROS node to adjust the robot torso from
its current position to the input
Input: self <Object>, pose <Array>
Return: perform movement
'''
joint_names = [
'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint',
'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
]
poses = [
pose, 1.3192424769714355, 1.4000714648620605, -0.20049656002880095,
1.5290160491638183, -0.0004613047506046297, 1.660243449769287,
-0.00012475593825578626
]
self.move_group.moveToJointPosition(joint_names, poses,
wait=False) # plan
self.move_group.get_move_action().wait_for_result()
return self.move_group.get_move_action().get_result()
def look_at(self, frame_id, x, y, z):
'''
Description: sends the ROS node a goal position to move
the head to face
Input: self <Object>, frame_id <Int>, x <Int>, y <Int>, z <Int>
Return: None
'''
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = frame_id
goal.target.point.x = x
goal.target.point.y = y
goal.target.point.z = z
goal.pointing_frame = "pointing_frame"
goal.pointing_axis.x = 1
goal.pointing_axis.y = 0
goal.pointing_axis.z = 0
# send goal
self.point_head_client.send_goal(goal)
def camera_info_callback(self, data):
'''
Description: resets the current camera info to the stored data
Input: self <Object>, data <Object>
Return: None
'''
self.camera_info = data
def key_callback(self, keypress):
'''
Description: sends a signal to cancel the current navigation goal
Input: self <Object>, keypress <Object>
Return: None
'''
if keypress.data == "k":
self.base_client.cancel_goal()
def point_published_callback(self, data, point_id):
'''
Description: registers a point in the ROS node
Input: self <Object>, data <Object>, point_id <Int>
Return: None
'''
self.points_registered.add(point_id)
self.published_points[point_id][0] = data.point.x
self.published_points[point_id][1] = data.point.y
self.published_points[point_id][2] = data.point.z
def sample_position(self, x_center, y_center, sample_max_radius,
sample_min_radius):
'''
Description: generates a random position within a radius as a goal point
for the robot
Input: self <Object>, x_center <Int>, y_center <Int>,
sample_max_radius <Int>, sample_min_radius <Int>
Return: generated position <Array>
'''
min_x = x_center - sample_max_radius
max_x = x_center - sample_min_radius * math.sin(.1745)
sampled_theta = random.random() * (2 * math.pi)
sampled_r = random.random() * (sample_max_radius -
sample_min_radius) + sample_min_radius
sampled_x = sampled_r * math.cos(sampled_theta) + x_center
sampled_y = sampled_r * math.sin(sampled_theta) + y_center
sampled_z = random.random() * (
self.max_spine_height -
self.min_spine_height) + self.min_spine_height
x_diff = sampled_x - x_center
y_diff = sampled_y - y_center
theta = math.atan2(y_diff, x_diff) + math.pi
if theta > (2 * math.pi):
theta = theta - (2 * math.pi)
if theta < 0:
theta = theta + (2 * math.pi)
position = [sampled_x, sampled_y, theta, sampled_z]
return position
def get_img(self):
return self.img
def get_pc(self):
return self.pc
def rviz_point_tl_tf(self):
return self.rviz_point_tl_tf
def rviz_point_br_tf(self):
return self.rviz_point_br_tf
class Robot(object):
"""
fetch max x = 0.96 (wrist roll joint)
"""
def __init__(self):
rospy.loginfo("Waiting for Fetch ...")
self.tuck_the_arm_joints_value = [
0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0
]
self.stow_joints_value = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso")
self.joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
def move_to_pose(self, postion, quaternion, frame='base_link'):
target_frame = 'wrist_roll_link'
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = Pose(
Point(postion[0], postion[1], postion[2]),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
if not rospy.is_shutdown():
self.move_group.moveToPose(pose_stamped, target_frame)
result = self.move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Move success!")
return True
else:
rospy.logerr("in state: %s",
self.move_group.get_move_action().get_state())
return False
else:
rospy.logerr("MoveIt! failure no result returned.")
return False
def move_to_joints(self, joints_value):
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names,
joints_value,
wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("move to joint Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
def tuck_the_arm(self):
self.move_to_joints(self.tuck_the_arm_joints_value)
def stow(self):
self.move_to_joints(self.stow_joints_value)
def stop(self):
"""This stops all arm movement goals
It should be called when a program is exiting so movement stops
"""
self.move_group.get_move_action().cancel_all_goals()
class GripperController(object):
def __init__(self):
self.client = actionlib.SimpleActionClient(
"gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_action...")
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.goal = GripperCommandGoal()
self.client.wait_for_server()
def close(self):
'''Close the gripper'''
self.goal.command.position = 0
self.goal.command.max_effort = 100
self.client.send_goal(self.goal)
self.client.wait_for_result()
def release(self):
'''Release the gripper'''
self.goal.command.position = 1
self.goal.command.max_effort = 100
self.client.send_goal(self.goal)
self.client.wait_for_result()
def tuck(self):
'''Tuck the arm of fetch'''
position = (0.0555, -0.138, 0.571)
quaternion = (0.45848, -0.50438, 0.5078, 0.5268)
self.move_to_pose(position, quaternion)
def move_to_pose(self, postion, quaternion, frame='base_link'):
target_frame = 'wrist_roll_link'
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = Pose(
Point(postion[0], postion[1], postion[2]),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
if not rospy.is_shutdown():
self.move_group.moveToPose(pose_stamped, target_frame)
result = self.move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Move success!")
else:
rospy.logerr("in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# self.move_group.get_move_action().cancel_all_goals()
def move_to_some_pose(self, postion, direction, frame='base_link'):
target_frame = 'wrist_roll_link'
quatrenion = Transformer().quaternion_from_direction((1, 0, 0),
direction)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = Pose(Point(postion[0], postion[1], postion[2]),
quatrenion)
if not rospy.is_shutdown():
self.move_group.moveToPose(pose_stamped, target_frame)
result = self.move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("See!")
else:
rospy.logerr("in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
self.move_group.get_move_action().cancel_all_goals()
def wave(self):
'''Just for fun'''
gripper_poses = [
((0.347, 0.045, 1.022), (-0.174, -0.301, 0.273, 0.635)),
((0.347, 0.245, 1.022), (-0.274, -0.701, 0.173, 0.635))
]
for i in range(5):
for pose in gripper_poses:
self.move_to_pose(pose[0], pose[1])
self.stop()
def stop(self):
"""This stops all arm movement goals
It should be called when a program is exiting so movement stops
"""
self.move_group.get_move_action().cancel_all_goals()
class FetchClient(object):
def __init__(self):
self.bfp = True
self.robot = robot_interface.Robot_Interface()
self.url = 'http://172.31.76.30:80/ThinkingQ/'
self.joint_names = ["shoulder_pan_joint",
"shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"]
self.tf_listener = TransformListener()
trfm = Transformer()
self.r2b = trfm.transform_matrix_of_frames(
'head_camera_rgb_optical_frame', 'base_link')
self.model = load_model("./model/0.988.h5")
self.br = TransformBroadcaster()
self.move_group = MoveGroupInterface("arm", "base_link")
# self.pose_group = moveit_commander.MoveGroupCommander("arm")
self.cam = camera.RGBD()
self.position_cloud = None
while True:
try:
cam_info = self.cam.read_info_data()
if cam_info is not None:
break
except:
rospy.logerr('camera info not recieved')
self.pcm = PCM()
self.pcm.fromCameraInfo(cam_info)
def transform_matrix_of_frames(self, source_frame, target_frame):
if self.tf_listener.frameExists(source_frame) and self.tf_listener.frameExists(target_frame):
t = self.tf_listener.getLatestCommonTime(
source_frame, target_frame)
# Compute the transform from source to target
position, quaternion = self.tf_listener.lookupTransform(
target_frame, source_frame, t)
transfrom_matrix = transformations.quaternion_matrix(quaternion)
transfrom_matrix[0:3, 3] = position
return transfrom_matrix
else:
return None
def get_corner(self):
print "get_corner"
html = ""
while html=="":
image_time = time.time()
img = self.cam.read_color_data()
while img is None:
img = self.cam.read_color_data()
cv2.imwrite("image/fetch.jpg", img)
files = {'IMAGE': (str(image_time) + '.jpg', open('image/fetch.jpg', 'rb'), 'image/png', {})}
response = requests.request("POST", url=self.url, files=files, timeout=15000)
html = response.text
print html
exit(0)
a = html.split("\n")[1:-1]
# print a
pl = np.array([int(i) for i in a])
# print pl
re = []
i = 0
while i < len(a):
point = []
# print "pl i", pl[i]
point.append(pl[i])
point.append(pl[i+1])
re.append(point)
i = i+2
# print re
return re
def convert_camera_position_to_base_position(self, array):
print 'r2b', self.r2b
ret = np.dot(self.r2b[0:3, 0:3], array) # Rotate
ret = ret + self.r2b[0:3, 3]
return ret
def update_position_cloud(self):
pc = self.robot.camera.read_point_cloud()
arr = pypcd.numpy_pc2.pointcloud2_to_array(pc, split_rgb=True)
self.position_cloud = pypcd.numpy_pc2.get_xyz_points(arr, remove_nans=False)
def get_position_by_pix(self, x, y):
pc = self.robot.camera.read_point_cloud()
arr = pypcd.numpy_pc2.pointcloud2_to_array(pc, split_rgb=True)
po = pypcd.numpy_pc2.get_xyz_points(arr, remove_nans=False)
return po[x, y]
def broadcast_position(self, position, name):
# while self.bfp:
# print "111"
a = transformations.quaternion_from_euler(
ai=-2.355, aj=-3.14, ak=0.0)
b = transformations.quaternion_from_euler(
ai=0.0, aj=0.0, ak=1.57)
base_rot = transformations.quaternion_multiply(a, b)
self.br.sendTransform(position,
base_rot,
rospy.Time.now(),
name,
'head_camera_rgb_optical_frame')
def get_position_of_baselink(self, name):
listener = TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
fetch_client.broadcast_position(cam_position, name)
pose, rot = listener.lookupTransform('base_link', name, rospy.Time(0))
except Exception as e:
print e
continue
rate.sleep()
if pose:
return pose
def show_image(self, im):
plt.imshow(im)
pylab.show()
def move_to_position(self, position):
position[2] -= 0.24
x = np.round(np.array([position]), 4) * 100
print self.model.predict(x)
joints = np.round(self.model.predict(x)[0], 4) / 100
print joints
joint_3 = 0 - joints[1] - joints[2] + 1.5
disco_poses = [
[joints[0], joints[1], 0.0, joints[2], 0.0, joint_3, 1.5+joints[0]],# 1 close gripper
]
for pose in disco_poses:
if rospy.is_shutdown():
break
print pose
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Reach success!")
# position = self.pose_group.get_current_pose("wrist_roll_link").pose.position
# print(position)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Reach fail")
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
# self.move_group.get_move_action().cancel_all_goals()
def move_to_position_up(self, position):
position[2] -= 0.24
x = np.round(np.array([position]), 4) * 100
print self.model.predict(x)
joints = np.round(self.model.predict(x)[0], 4) / 100
print joints
joint_3 = 0 - joints[1] - joints[2] + 1.5
disco_poses = [
[joints[0], joints[1], 0.0, joints[2], 0.0, joint_3 , 0-joints[1]-joints[2]], #up
]
for pose in disco_poses:
if rospy.is_shutdown():
break
print pose
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Reach success!")
# position = self.pose_group.get_current_pose("wrist_roll_link").pose.position
# print(position)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Reach fail")
else:
rospy.logerr("MoveIt! failure no result returned.")
def move_to_position_with_up(self, position):
position[2] -= 0.24
x = np.round(np.array([position]), 4) * 100
print self.model.predict(x)
joints = np.round(self.model.predict(x)[0], 4) / 100
print joints
joint_3 = 0 - joints[1] - joints[2] + 1.5
disco_poses = [
[joints[0], joints[1], 0.0, joints[2] - 0.6, 0.0, joint_3 + 0.6, 1.5+joints[0]], #up
[joints[0], joints[1], 0.0, joints[2], 0.0, joint_3, 1.5+joints[0]],# 1 close gripper
]
for pose in disco_poses:
if rospy.is_shutdown():
break
print pose
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Reach success!")
# position = self.pose_group.get_current_pose("wrist_roll_link").pose.position
# print(position)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Reach fail")
else:
rospy.logerr("MoveIt! failure no result returned.")
def stow(self):
disco_poses = [
[-0.0, -0.7, 0.0, 0.5, 0.0, 1.5, 1.3],
[1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]#stow
]
for pose in disco_poses:
if rospy.is_shutdown():
break
print pose
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Reach success!")
if disco_poses.index(pose) == 1:
self.robot.open_gripper()
# position = self.pose_group.get_current_pose("wrist_roll_link").pose.position
# print(position)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Reach fail")
else:
rospy.logerr("MoveIt! failure no result returned.")
def move_corner(self, x, y):
position = self.cal_position.get_base_position_from_pix(x, y)
position[0] = position[0] - 0.20
move_group = MoveGroupInterface("arm_with_torso", "base_link")
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'
pose = Pose(Point(position[0], position[1], position[2]),
Quaternion(0, 0, 0, 1))
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
# Finish building the Pose_stamped message
# If the message stamp is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
# Move gripper frame to the pose specified
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Hello there!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
time.sleep(1)
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
move_group.moveToJointPosition(joint_names, joints_value, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("pose Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
