def move_arm(self, positions):
self._arm_goal = copy.deepcopy(positions)
jp = YoubotProxy.make_brics_msg_arm(self.arm_num, positions)
rospy.logdebug("brics message created")
rospy.logdebug(jp)
r = rospy.Rate(50)
t0 = rospy.Time().now()
pubbed = False
while not rospy.is_shutdown():
curr_jpos = self._joint_positions_arm
d = BaseProxy.measure_joint_distance_sum(curr_jpos, positions)
v = BaseProxy.measure_joint_velocity_ss(self._joint_velocities_arm)
rospy.logdebug(d)
rospy.logdebug("arm joint positions")
rospy.logdebug(curr_jpos)
rospy.logdebug(d)
rospy.logdebug("arm joint velocities")
rospy.logdebug(self._joint_velocities_arm)
if ((d < self.arm_joint_distance_tol) and (v<=self.arm_joint_velocity_tol)):
# rospy.logdebug("moved arm to joint position error of: " + str(d))
break
td = rospy.Time().now()
if (td-t0 > self.arm_move_duration):
rospy.logerr("move arm timeout")
raise Exception("move arm timeout")
break
self._arm_pub.publish(jp)
# if not pubbed:
# self._arm_pub.publish(jp)
# pubbed = True
r.sleep()
def move_arm(self, positions):
self._arm_goal = copy.deepcopy(positions)
jp = YoubotProxy.make_brics_msg_arm(self.arm_num, positions)
rospy.logdebug("brics message created")
rospy.logdebug(jp)
r = rospy.Rate(50)
t0 = rospy.Time().now()
pubbed = False
while not rospy.is_shutdown():
curr_jpos = self._joint_positions_arm
d = BaseProxy.measure_joint_distance_sum(curr_jpos, positions)
v = BaseProxy.measure_joint_velocity_ss(self._joint_velocities_arm)
rospy.logdebug(d)
rospy.logdebug("arm joint positions")
rospy.logdebug(curr_jpos)
rospy.logdebug(d)
rospy.logdebug("arm joint velocities")
rospy.logdebug(self._joint_velocities_arm)
if ((d < self.arm_joint_distance_tol)
and (v <= self.arm_joint_velocity_tol)):
# rospy.logdebug("moved arm to joint position error of: " + str(d))
break
td = rospy.Time().now()
if (td - t0 > self.arm_move_duration):
rospy.logerr("move arm timeout")
raise Exception("move arm timeout")
break
self._arm_pub.publish(jp)
# if not pubbed:
# self._arm_pub.publish(jp)
# pubbed = True
r.sleep()
