diff --git a/src/ur5_rtt_gazebo_component.cpp b/src/ur5_rtt_gazebo_component.cpp
index 43137e1..9286c5b 100644
--- a/src/ur5_rtt_gazebo_component.cpp
+++ b/src/ur5_rtt_gazebo_component.cpp
@@ -398,7 +398,7 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
 
 		// Create vector containing awaited position.
 		RealVectorPtr inputdata = RealVector::create(elm->getInputDimension(), 0.0);
-		for (int j=0; j<inputdata->getDimension(); j++) inputdata->setValueEquals(j,target_value[j]);
+		for (int j=0; j<inputdata->getDimension(); j++) inputdata->setValueEquals(j,model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian());
 
 		// Create vector containing the torque which should be applied.
 		RealVectorPtr result = elm->evaluate(inputdata);