Created
January 25, 2018 17:46
-
-
Save BryceStevenWilley/c768d16effcb5b3e5408f20ead886be0 to your computer and use it in GitHub Desktop.
Rospy script to replicate chomp obstacle error.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
import rospy | |
import std_msgs.msg | |
from sensor_msgs.msg import JointState | |
from moveit_msgs.msg import RobotState | |
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander | |
import geometry_msgs.msg | |
import time | |
def load_scene(): | |
scene = PlanningSceneInterface() | |
# clear the scene | |
scene.remove_world_object() | |
robot = RobotCommander() | |
# pause to wait for rviz to load | |
rospy.sleep(4) | |
p = geometry_msgs.msg.PoseStamped() | |
p.header.frame_id = robot.get_planning_frame() | |
p.header.stamp = rospy.Time.now() | |
p.pose.position.x = 0.68 | |
p.pose.position.y = 0.3 | |
p.pose.position.z = 1.25 | |
p.pose.orientation.x = 0.0 | |
p.pose.orientation.y = 0.0 | |
p.pose.orientation.z = 0.0 | |
p.pose.orientation.w = 1.0 | |
scene.add_box('box', p, (0.15, 0.15, 0.15)) | |
rospy.sleep(0.2) | |
def make_plan(): | |
group = MoveGroupCommander('manipulator') | |
joint_state = JointState() | |
joint_state.header = std_msgs.msg.Header() | |
joint_state.header.stamp = rospy.Time.now() | |
joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] | |
joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] | |
moveit_robot_state = RobotState() | |
moveit_robot_state.joint_state = joint_state | |
group.set_start_state(moveit_robot_state) | |
current_joints = group.get_current_joint_values() | |
current_joints[0] = 1.0 | |
group.set_joint_value_target(current_joints) | |
plan = group.plan() | |
rospy.sleep(5) | |
if __name__ == '__main__': | |
rospy.init_node('test_chomp', anonymous=True) | |
while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): | |
time.sleep(0.5) | |
load_scene() | |
make_plan() | |
rospy.spin() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment