from moveit_msgs.msg import Constraints
from moveit_msgs.msg import MoveGroupAction
from moveit_msgs.msg import MoveGroupGoal
from moveit_msgs.msg import MoveGroupActionGoal
from moveit_msgs.msg import MotionPlanRequest
from geometry_msgs.msg import Vector3
if __name__ == '__main__':
rospy.init_node('moveit_publisher')
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = "/BASE"
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = "/BASE"
start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"]
start_state.joint_state.position = [-0.2569046038066598, -0.8442722962923348, 1.849082034218144, 0.26825374068443164, -0.04090683809444329, 5.745512865657193]
start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jc2 = JointConstraint()
from moveit_msgs.msg import JointConstraint
from moveit_msgs.msg import Constraints
from moveit_msgs.msg import MoveGroupAction
from moveit_msgs.msg import MoveGroupGoal
from moveit_msgs.msg import MoveGroupActionGoal
from moveit_msgs.msg import MotionPlanRequest
from geometry_msgs.msg import Vector3
if __name__ == '__main__':
rospy.init_node('moveit_publisher')
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = "/BASE"
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = "/BASE"
start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"]
start_state.joint_state.position = [
-0.2569046038066598, -0.8442722962923348, 1.849082034218144,
0.26825374068443164, -0.04090683809444329, 5.745512865657193
]
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s"%e
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s" % e
