How can I move the robot using MoveGroupCommander
I am trying to write my pick-and-place python script using the color_recognition.py file as a reference. However, I feel like the XArmCtrl class does not plan a path and move near an object using a predefined path. I want to confirm my doubt. Therefore, please correct me if I am wrong. How can I use it to move to the point in the 3D coordinate?
Below is my code:
class XArmCtrl(object):
def __init__(self, dof):
self._commander = moveit_commander.move_group.MoveGroupCommander(
"xarm{}".format(dof)
)
self._init()
def _init(self):
pass
def set_joints(self, angles, wait=True):
try:
joint_target = self._commander.get_current_joint_values()
print(joint_target)
for i in range(joint_target):
if i >= len(angles):
break
if angles[i] is not None:
joint_target[i] = math.radians(angles[i])
print("set_joints, joints={}".format(joint_target))
self._commander.set_joint_value_target(joint_target)
ret = self._commander.go(wait=wait)
print("move to finish, ret={}".format(ret))
return ret
except Exception as e:
print("[Ex] set_joints exception, ex={}".format(e))
def set_joint(self, angle, inx=-1, wait=True):
try:
joint_target = self._commander.get_current_joint_values()
joint_target[inx] = math.radians(angle)
print("set_joints, joints={}".format(joint_target))
self._commander.set_joint_value_target(joint_target)
ret = self._commander.go(wait=wait)
print("move to finish, ret={}".format(ret))
return ret
except Exception as e:
print("[Ex] set_joint exception, ex={}".format(e))
return False
def moveto(
self,
x=None,
y=None,
z=None,
ox=None,
oy=None,
oz=None,
relative=False,
wait=True,
):
if (
x == 0
and y == 0
and z == 0
and ox == 0
and oy == 0
and oz == 0
and relative
):
return True
try:
pose_target = self._commander.get_current_pose().pose
if relative:
pose_target.position.x += x / 1000.0 if x is not None else 0
pose_target.position.y += y / 1000.0 if y is not None else 0
pose_target.position.z += z / 1000.0 if z is not None else 0
pose_target.orientation.x += ox if ox is not None else 0
pose_target.orientation.y += oy if oy is not None else 0
pose_target.orientation.z += oz if oz is not None else 0
else:
pose_target.position.x = x / 1000.0 if x is not None else pose_target.position.x
pose_target.position.y = y / 1000.0 if y is not None else pose_target.position.y
pose_target.position.z = z / 1000.0 if z is not None else pose_target.position.z
pose_target.orientation.x = ox if ox is not None else pose_target.orientation.x
pose_target.orientation.y = oy if oy is not None else pose_target.orientation.y
pose_target.orientation.z = oz if oz is not None else pose_target.orientation.z
print(
"move to position=[{:.2f}, {:.2f}, {:.2f}], orientation=[{:.6f}, {:.6f}, {:.6f}]".format(
pose_target.position.x * 1000.0,
pose_target.position.y * 1000.0,
pose_target.position.z * 1000.0,
pose_target.orientation.x,
pose_target.orientation.y,
pose_target.orientation.z,
)
)
self._commander.set_pose_target(pose_target)
ret = self._commander.go(wait=wait)
print("move to finish, ret={}".format(ret))
return ret
except Exception as e:
print("[Ex] moveto exception: {}".format(e))
return False
What does it mean? Can you please elaborate a bit more?
I suggest going through MoveIt Tutorials. After finishing the official MoveIt documentation, you may check ravijo/baxter_moveit_tutorial to get an idea of how does MoveIt work. For example, after instantiating
moveit_commander.MoveGroupCommander( ... )
, you may callgroup.set_pose_target( ... )
and so on. However, please do not skip the official MoveIt documentation.For any future people who is seeking the answer to my question, you can use - https://ros-planning.github.io/moveit...