def jnt_state_cb(self, msg):
for name, pos, vel in zip(msg.name, msg.position, msg.velocity):
if name == self.name:
self.vel = 0.5 * self.vel + 0.5 * vel
if not self.initialized:
self.pos_desi = pos
self.initialized = True
cmd = JointCommand()
cmd.name = self.name
cmd.effort = 190.0 * (self.pos_desi - pos) - 4.0 * self.vel
print 'Joint at %f, going to %f, commanding joint %f'%(pos,self.pos_desi, cmd.effort)
self.pub.publish(cmd)
def jnt_state_cb(self, msg):
for name, pos, vel in zip(msg.name, msg.position, msg.velocity):
if name == self.name:
self.vel = 0.5 * self.vel + 0.5 * vel
if not self.initialized:
self.pos_desi = pos
self.initialized = True
cmd = JointCommand()
cmd.name = self.name
cmd.effort = 190.0 * (self.pos_desi - pos) - 4.0 * self.vel
print 'Joint at %f, going to %f, commanding joint %f' % (
pos, self.pos_desi, cmd.effort)
self.pub.publish(cmd)
def jnt_state_cb(self, msg):
velocity = {}
for name, vel in zip(msg.name, msg.velocity):
velocity[name] = vel
# lowpass for measured velocity
self.vel_trans = 0.5 * self.vel_trans + 0.5 * (velocity[
self.r_joint] + velocity[self.l_joint]) * self.wheel_radius / 2.0
self.vel_rot = 0.5 * self.vel_rot + 0.5 * (
velocity[self.r_joint] - velocity[self.l_joint]
) * self.wheel_radius / (2.0 * self.wheel_basis)
# velocity commands
vel_trans = self.vel_trans_desi + self.k_trans * (self.vel_trans_desi -
self.vel_trans)
vel_rot = self.vel_rot_desi + self.k_rot * (self.vel_rot_desi -
self.vel_rot)
# wheel commands
l_cmd = JointCommand()
l_cmd.name = self.l_joint
l_cmd.effort = self.vel_to_eff * (
vel_trans / self.wheel_radius -
vel_rot * self.wheel_basis / self.wheel_radius)
self.pub.publish(l_cmd)
r_cmd = JointCommand()
r_cmd.name = self.r_joint
r_cmd.effort = self.vel_to_eff * (
vel_trans / self.wheel_radius +
vel_rot * self.wheel_basis / self.wheel_radius)
self.pub.publish(r_cmd)
def jnt_state_cb(self, msg):
if self.isActive:
if not self.initialized:
self.vel_rot_desi = 0.0
self.vel_trans_desi = 0.0
self.initialized = True
velocity = {}
for name, vel in zip(msg.name, msg.velocity):
velocity[name] = vel
# lowpass for measured velocity
self.vel_trans = 0.5*self.vel_trans + 0.5*(velocity[self.r_joint] + velocity[self.l_joint])*self.wheel_radius/2.0
self.vel_rot = 0.5*self.vel_rot + 0.5*(velocity[self.r_joint] - velocity[self.l_joint])*self.wheel_radius/(2.0*self.wheel_basis)
# velocity commands
vel_trans = self.vel_trans_desi + self.k_trans*(self.vel_trans_desi - self.vel_trans)
vel_rot = self.vel_rot_desi + self.k_rot*(self.vel_rot_desi - self.vel_rot)
# wheel commands
l_cmd = JointCommand()
l_cmd.name = self.l_joint
l_cmd.effort = self.vel_to_eff*(vel_trans/self.wheel_radius - vel_rot*self.wheel_basis/self.wheel_radius)
self.pub.publish(l_cmd)
r_cmd = JointCommand()
r_cmd.name = self.r_joint
r_cmd.effort = self.vel_to_eff*(vel_trans/self.wheel_radius + vel_rot*self.wheel_basis/self.wheel_radius)
self.pub.publish(r_cmd)
def execute_cb(self, msg):
for i in range(0, len(msg.joint_names_goal)):
cmd = JointCommand()
cmd.name = msg.joint_names_goal[i]
cmd.effort = self.efforts[msg.joint_names_goal[i]]
rospy.logwarn(str(msg.joint_names_goal[i]))
rospy.logwarn(str(msg.joint_position_angles_goal[i]))
if msg.joint_position_angles_goal[i] < self.joint_positions[
msg.joint_names_goal[i]]:
cmd.effort = cmd.effort * -1
rospy.logwarn('publishe ' + str(cmd.effort) + ' auf ' +
str(cmd.name))
self.arm_joint_pub.publish(cmd)
while True:
if abs(msg.joint_position_angles_goal[i] -
self.joint_positions[msg.joint_names_goal[i]]
) > self.goal_threshold:
rospy.logwarn(
str(
abs(msg.joint_position_angles_goal[i] -
self.joint_positions[msg.joint_names_goal[i]]))
)
continue
else:
cmd.effort = 0.0
self.arm_joint_pub.publish(cmd)
time.sleep(.1)
self.arm_joint_pub.publish(cmd)
break
rospy.logwarn('reached')
self._result.joint_position_angles_result = [
self.joint_positions[MOTOR_A], self.joint_positions[MOTOR_B],
self.joint_positions[GRIPPER]
]
self._sas.set_succeeded(self._result)
rospy.logwarn("ArmPositionServer succeeded")
def jnt_state_cb(self, msg):
for name, pos, vel in zip(msg.name, msg.position, msg.velocity):
if name == self.motor1name:
if not self.motor1initialized:
self.motor1pos_desi = pos
self.motor1initialized = True
self.motor1error = self.motor1pos_desi - pos
self.motor1delta_err = self.motor1prev_err - self.motor1error
self.motor1integral_err += self.motor1error
if self.motor1integral_err > 0.5:
self.motor1integral_err = 0.5
if self.motor1integral_err < -0.5:
self.motor1integral_err = -0.5
self.motor1p_out = self.motor1error * self.motor1KP
self.motor1i_out = self.motor1integral_err * self.motor1KI
self.motor1d_out = self.motor1delta_err * self.motor1KD
self.motor1output = self.motor1p_out + self.motor1i_out + self.motor1d_out
if self.motor1output > 0.65:
self.motor1output = 0.65
if self.motor1output < -0.65:
self.motor1output = -0.65
self.motor1prev_err = self.motor1error
cmd = JointCommand()
cmd.name = self.motor1name
cmd.effort = self.motor1output ;
#print 'Motor Joint 1 at %f, going to %f, commanding joint %f'%(pos,self.motor1pos_desi, cmd.effort)
self.pub.publish(cmd)
if name == self.motor2name:
if not self.motor2initialized:
self.motor2pos_desi = pos
self.motor2initialized = True
self.motor2error = self.motor2pos_desi - pos
self.motor2delta_err = self.motor2prev_err - self.motor2error
self.motor2integral_err += self.motor2error
if self.motor2integral_err > 0.5:
self.motor2integral_err = 0.5
if self.motor2integral_err < -0.5:
self.motor2integral_err = -0.5
self.motor2p_out = self.motor2error * self.motor2KP
self.motor2i_out = self.motor2integral_err * self.motor2KI
self.motor2d_out = self.motor2delta_err * self.motor2KD
self.motor2output = self.motor2p_out + self.motor2i_out + self.motor2d_out
if self.motor2output > 0.65:
self.motor2output = 0.65
if self.motor2output < -0.65:
self.motor2output = -0.65
self.motor2prev_err = self.motor2error
cmd = JointCommand()
cmd.name = self.motor2name
cmd.effort = self.motor2output ;
#print 'Motor Joint 2 at %f, going to %f, commanding joint %f'%(pos,self.motor2pos_desi, cmd.effort)
self.pub.publish(cmd)
if name == self.motor3name:
if not self.motor3initialized:
self.motor3pos_desi = pos
self.motor3initialized = True
self.motor3error = self.motor3pos_desi - pos
self.motor3delta_err = self.motor3prev_err - self.motor3error
self.motor3integral_err += self.motor3error
if self.motor3integral_err > 0.5:
self.motor3integral_err = 0.5
if self.motor3integral_err < -0.5:
self.motor3integral_err = -0.5
self.motor3p_out = self.motor3error * self.motor3KP
self.motor3i_out = self.motor3integral_err * self.motor3KI
self.motor3d_out = self.motor3delta_err * self.motor3KD
self.motor3output = self.motor3p_out + self.motor3i_out + self.motor3d_out
if self.motor3output > 0.65:
self.motor3output = 0.65
if self.motor3output < -0.65:
self.motor3output = -0.65
self.motor3prev_err = self.motor3error
cmd = JointCommand()
cmd.name = self.motor3name
cmd.effort = self.motor3output ;
#print 'Motor Joint 3 at %f, going to %f, commanding joint %f'%(pos,self.motor3pos_desi, cmd.effort)
self.pub.publish(cmd)
#import roslib; roslib.load_manifest('nxt_controllers')
import rospy
import math
import thread
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist
from nxt_msgs.msg import Range, JointCommand, Contact
import time
import unittest
import multiprocessing
import threading
MOTOR_A = "motor_1"
MOTOR_B = "motor_2"
GRIPPER = "motor_3"
JOINT_0_COMMAND = JointCommand()
JOINT_1_COMMAND = JointCommand()
GRIPPER_COMMAND = JointCommand()
"""
Since during the test cases the "real" system is not running,
and thus also ROS is not running, all ROS relevant data be mocked.
For this purpose, the function pointers of the public and sleep functions
are overwritten with "empty" functions
"""
class mock_ros:
def publish(self, data):
pass
def sleep(self):
