def motion(self, x, y, z, roll, pitch, yaw):
manipu_init_pose = self.manipu.get_current_pose().pose
self.x = x
self.y = y
self.z = z + 0.006
self.roll = math.radians(roll)
self.pitch = math.radians(pitch)
self.yaw = math.radians(yaw)
self.q = tf.transformations.quaternion_from_euler(
self.roll, self.pitch, self.yaw)
self.target_pose.position.x = self.x
self.target_pose.position.y = self.y
self.target_pose.position.z = self.z
self.target_pose.orientation.x = self.q[0]
self.target_pose.orientation.y = self.q[1]
self.target_pose.orientation.z = self.q[2]
self.target_pose.orientation.w = self.q[3]
self.manipu.set_pose_target(self.target_pose)
plan = self.manipu.plan()
if self.manipu.execute(plan):
print "This plan is succes!"
joint_msg = rospy.Publisher('/robotis/base/joint_pose_msg',
JointPose,
queue_size=100)
jointpose = JointPose()
jointpose.name = plan.joint_trajectory.joint_names
start = time.time()
jointpose.value = plan.joint_trajectory.points[0].positions
joint_msg.publish(jointpose)
plan_count = 1
while (plan_count < len(plan.joint_trajectory.points)):
if ((time.time() - start) >= 0.8):
jointpose.value = plan.joint_trajectory.points[
plan_count].positions
joint_msg.publish(jointpose)
plan_count = plan_count + 1
start = time.time()
print "finished to send all jointTrajectory"
else:
print "Thin plan is NOT succes"
return
def jointMove(self, speed, slide_pos=0, cmd=[0, 0, 0, 0, 0, 0, 0]):
"""Publish msg of joint cmd (rad) to manager node."""
name = list()
value = list()
for i, val in enumerate(cmd):
name.append('joint{}'.format(i + 1))
value.append(radians(val))
self.__joint_pub.publish(self.joint_topic,
JointPose(name, value, slide_pos, speed))
def jointMove(self, slide_pos = 0,cmd=[0, 0, 0, 0, 0, 0, 0]):
"""Publish msg of joint cmd (rad) to manager node."""
name = list()
value = list()
speed = self.__speed
for i, val in enumerate(cmd):
name.append('joint{}'.format(i+1))
value.append(val)
self.__joint_pub.publish(JointPose(name, value, slide_pos, speed))
self.__is_busy = True