Совместные ограничения Ros для пакета Moveit - PullRequest
Новая
       17

Совместные ограничения Ros для пакета Moveit

0 голосов
/ 15 мая 2019

У меня есть робот UR5 и я пытаюсь сделать первые 3 сустава устойчивыми при изменении положения манипулятора. Но когда я устанавливаю совместные ограничения на 0, пакет moveit не дает мне решения: 

from universal_robot_kinematics import *
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from tf.transformations import euler_from_quaternion, quaternion_from_euler


moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_manipulator', anonymous=True)

robot = moveit_commander.RobotCommander()

scene = moveit_commander.PlanningSceneInterface()

group = moveit_commander.MoveGroupCommander("manipulator")

display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)

## Get Joint Position list
joint_goal = group.get_current_joint_values()

print("First of the Joint Goals: ", joint_goal)

## Set Constraints of the system joints

constraint_0 = moveit_msgs.msg.JointConstraint()

constraint_0.joint_name = 'shoulder_pan_joint'
constraint_0.position = joint_goal[0]
constraint_0.tolerance_above = 0
constraint_0.tolerance_below = 0
constraint_0.weight = 1

constraint_1 = moveit_msgs.msg.JointConstraint()

constraint_1.joint_name = 'shoulder_lift_joint'
constraint_1.position = joint_goal[1]
constraint_1.tolerance_above = 0
constraint_1.tolerance_below = 0
constraint_1.weight = 2

constraint_2 = moveit_msgs.msg.JointConstraint()

constraint_2.joint_name = 'elbow_joint'
constraint_2.position = joint_goal[2]
constraint_2.tolerance_above = 0
constraint_2.tolerance_below = 0
constraint_2.weight = 3

## Append the system constraints

joint_limits = moveit_msgs.msg.Constraints()
joint_limits.joint_constraints.append(constraint_0)
joint_limits.joint_constraints.append(constraint_1)
joint_limits.joint_constraints.append(constraint_2)

## Set group Constraints

group.set_path_constraints(joint_limits)

rpy = group.get_current_rpy()

## Set pose target

group.set_rpy_target([rpy[0], rpy[1] , rpy[2] + 1*(pi/2)], end_effector_link="wrist_3_link")

plan = group.plan()

plan = group.go(wait=True)

display_trajectory = moveit_msgs.msg.DisplayTrajectory()

display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);

print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)

Здесь, в этом коде, я приказываю своему роботу перемещать манипулятор, одновременно устанавливая первые 3 соединения стабильными. Как вы думаете, это правильный способ сделать это? Пожалуйста, есть идеи, как сделать первые три соединения устойчивыми при изменении ориентации манипулятора?

...