changing values

Report crashes, strange behaviour, or apparent bugs
Post Reply
Elmo
Posts: 31
Joined: 13 Oct 2016, 16:34

changing values

Post by Elmo »

Code: Select all

    srvObHandle = rospy.ServiceProxy('/vrep/simRosGetObjectHandle',simRosGetObjectHandle)
    srvPose = rospy.ServiceProxy('/vrep/simRosGetObjectPose',simRosGetObjectPose)
    srvSetPose = rospy.ServiceProxy('/vrep/simRosSetObjectPose',simRosSetObjectPose)
    srvSetPosition = rospy.ServiceProxy('/vrep/simRosSetObjectPosition',simRosSetObjectPosition)
    

Code: Select all

  
 p2 = srvPose(boxHandle,-1).pose.pose.position
    p2=[p2.x,p2.y,p2.z]
    print 'P2-vektor: ',p2
    #t = listener.getLatestCommonTime(boxHandle, "/map")
    #p2, quaternion2 = listener.lookupTransform(boxHandle, "/map", t)
     
    #lua: p1=simGetObjectPosition(vehicleReference,-1)
    vehicle_handle = srvObHandle('youBot_vehicleTargetPosition').handle
    p1 = srvPose(vehicle_handle,-1).pose.pose.position
    p1=[p1.x,p1.y,p1.z]
    print 'P1-vektor: ',p1

p1 has always the same value, but p2 always has an other y-value.
so why?
The original hanoi always work, but the rosCall gets different values each time and completely different to lua(german: Im Original .ttt lua Skript bekommt man von der Fkt immer die gleichen Werte zurück, der zugehörige ROS Service liefert jedoch für die rote Box ständig verschiedene y werte. Zudem liefert der ROS-Service grundsätzlich andere werte. )


Is the source Code for funktions like simGetObjectMatrix som where? so I can use it from outside V-rep?




appendix

LUA

Code: Select all

P2-vektor:   0.74999976158142     -1.3866442216681e-08     0.14999997615814
P1-vektor:   -6.8725348683074e-06     0.16641589999199     -0.00040899217128754
P-vektor:   0.95806756190365     -0.21258170474742     0.19213423857992

m getobj: 
1 -1.2830659557039e-07 6.7129903413843e-08 0.74999976158142
1.2830659557039e-07 1 -4.4416538003134e-08 -1.3866442216681e-08
-6.7129903413843e-08 4.4416545108561e-08 1 0.14999997615814

v: 1 1.2830659557039e-07 -6.7129903413843e-08 
score: 0.95806752173006 

v: -1.2830659557039e-07 1 4.4416545108561e-08 
score: -0.21258181913987 

v: 6.7129903413843e-08 -4.4416538003134e-08 1 
score: 0.19213431233704 
angle=math.atan2: 1.2830659557039e-07 

m: 
1 -1.2830659557039e-07 0 0.74999976158142
1.2830659557039e-07 1 -0 -1.3866442216681e-08
0 0 1 0.14999997615814
m, angle:
Python

Code: Select all

P2-vektor:   0.74999976158142     -1.3866442216681e-08     0.14999997615814
P1-vektor:   -6.8725348683074e-06     0.16641589999199     -0.00040899217128754
P-vektor:   0.95806756190365     -0.21258170474742     0.19213423857992

m getobj: 
1 -1.2830659557039e-07 6.7129903413843e-08 0.74999976158142
1.2830659557039e-07 1 -4.4416538003134e-08 -1.3866442216681e-08
-6.7129903413843e-08 4.4416545108561e-08 1 0.14999997615814

v: 1 1.2830659557039e-07 -6.7129903413843e-08 
score: 0.95806752173006 

v: -1.2830659557039e-07 1 4.4416545108561e-08 
score: -0.21258181913987 

v: 6.7129903413843e-08 -4.4416538003134e-08 1 
score: 0.19213431233704 
angle=math.atan2: 1.2830659557039e-07 

m: 
1 -1.2830659557039e-07 0 0.74999976158142
1.2830659557039e-07 1 -0 -1.3866442216681e-08
0 0 1 0.14999997615814
m, angle:
python 2

Code: Select all

Program started
Simulator already running value: result: 0
P2-vektor:  [0.7499819993972778, -5.401298403739929e-05, 0.1499999463558197]
P1-vektor:  [2.006388967856765e-06, 0.1662178784608841, 1.4901161193847656e-08]
	P-vektor:  [0.9581983088611162, -0.21243426048552944, 0.19164468651038344]
m getobj: 
[[ -1.00000000e+00  -1.71969440e-07  -2.99902358e-08   0.00000000e+00]
 [  1.71906911e-07  -9.99997852e-01   2.07265658e-03   0.00000000e+00]
 [ -3.03466050e-08   2.07265658e-03   9.99997852e-01   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e+00]]
v:  [-0.99999999999998423, 1.7190691122352101e-07, -3.0346605007779134e-08] 

score:  -0.958198351196 

v:  [-1.7196944006568471e-07, -0.99999785204503433, 0.0020726565771841637] 

score:  0.212830853025 

anle=math.atan2: -1.71906911224e-07
m 
[[  1.00000000e+00   1.71906911e-07   0.00000000e+00   0.00000000e+00]
 [ -1.71906911e-07   1.00000000e+00   0.00000000e+00   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   1.00000000e+00   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e+00]] 
complete code

Code: Select all

#!/usr/bin/env python
# this python manage the tower of hanoi problem using ROS

import numpy as np
import std_msgs.msg
import tf
from tf import TransformListener
import geometry_msgs.msg
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose
import rospy
import roslib

#from vrep_common import simRosStartSimulation
import math
import time
#import vrep
import vrep_common
from vrep_common.srv import simRosStartSimulation
from vrep_common.srv import simRosStopSimulation
from vrep_common.srv import simRosGetObjectPose
from vrep_common.srv import simRosSetObjectPose
from vrep_common.srv import simRosGetObjectHandle
from vrep_common.srv import simRosSetObjectPosition
from vrep_common.srv import simRosGetJointMatrix




#def callback(data):
#    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)

#simGetObjectMatrix
def getTransformationMatrix(boxHandle):
    # epsilon for testing whether a number is close to zero
    EPS = np.finfo(float).eps * 4.0
    quaternion = srvPose(boxHandle,-1).pose.pose.orientation
    quaternion = [quaternion.x,quaternion.y,quaternion.z,quaternion.w]
    q = np.array(quaternion, dtype=np.float64, copy=True)
    n = np.dot(q, q)
    if n < EPS:
        return np.identity(4)
    q *= math.sqrt(2.0 / n)
    q = np.outer(q, q)
    return np.array([
        [1.0-q[2, 2]-q[3, 3],     q[1, 2]-q[3, 0],     q[1, 3]+q[2, 0], 0.0],
        [    q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3],     q[2, 3]-q[1, 0], 0.0],
        [    q[1, 3]-q[2, 0],     q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
        [                0.0,                 0.0,                 0.0, 1.0]])


# Param: listner all tfs, boxHandle searching
#def getBoxAdjustedMatrixAndFacingAngle(listener,boxHandle):
def getBoxAdjustedMatrixAndFacingAngle(boxHandle):       
    #lua: p2=simGetObjectPosition(boxHandle,-1)
    p2 = srvPose(boxHandle,-1).pose.pose.position
    p2=[p2.x,p2.y,p2.z]
    print 'P2-vektor: ',p2
    #t = listener.getLatestCommonTime(boxHandle, "/map")
    #p2, quaternion2 = listener.lookupTransform(boxHandle, "/map", t)
     
    #lua: p1=simGetObjectPosition(vehicleReference,-1)
    vehicle_handle = srvObHandle('youBot_vehicleTargetPosition').handle
    p1 = srvPose(vehicle_handle,-1).pose.pose.position
    p1=[p1.x,p1.y,p1.z]
    print 'P1-vektor: ',p1

    #t = listener.getLatestCommonTime("/base_link", "/map")
    #p1, quaternion1 = listener.lookupTransform("/base_link", "/map", t)

    #go on calulating the dirction and normalize
    p=[p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]]
    pl=math.sqrt(p[0]*p[0]+p[1]*p[1]+p[2]*p[2])
    p[0]=p[0]/pl
    p[1]=p[1]/pl
    p[2]=p[2]/pl
    
    print '	P-vektor: ', p

    #m=simGetObjectMatrix(boxHandle,-1)  or   m=GetObjectMatrix(boxHandle,"/map")
    m=getTransformationMatrix(boxHandle)
    
    print 'm getobj: \n',m
    #m = listener.fromTranslationRotation(position2,quaternion2)
    matchingScore=0
    for i in xrange(0,2):
        v=[m[0][i],m[1][i],m[2][i]] # go x,y,z ache
        print 'v: ',v,'\n'
        score=v[0]*p[0]+v[1]*p[1]+v[2]*p[2]
        print 'score: ',score, '\n'
        if (abs(score)>matchingScore):
            s=1
            if (score<0):
                s=-1
            matchingScore=abs(score)
            bestMatch=[v[0]*s,v[1]*s,v[2]*s]
    angle=math.atan2(bestMatch[1],bestMatch[0])
    print 'anle=math.atan2:', angle
    # m=simBuildMatrix(p2,{0,0,angle})
    #m = listener.fromTranslationRotation(p2,[0,0,angle,0])
    m[0][0] =  math.cos(angle)
    m[1][0] =  math.sin(angle)
    m[0][1] = -math.sin(angle)
    m[1][1] =  math.cos(angle)
    m[0][2] = 0
    m[2][0] = 0
    m[1][2] = 0
    m[2][1] = 0
    m[2][2] = 1
    m[3][3] = 1
    m[2][3] = 0
    m[3][2] = 0
    print 'm \n',m,'\n'
    return m, angle-math.pi/2

if __name__ == '__main__':
    print('Program started')
    ## Define Node and Topics give them time to get ready
    rospy.init_node("hanoi")
    
    # wait for important sevices of V-REP to come up
    rospy.wait_for_service("/vrep/simRosSetObjectPosition")
    rospy.wait_for_service("/vrep/simRosGetObjectPose")
    
    #start Simulator
    rospy.wait_for_service('/vrep/simRosStartSimulation')
    startSimulation = rospy.ServiceProxy('/vrep/simRosStartSimulation',simRosStartSimulation)
    resSimStart = startSimulation()  
    
    if -1 ==  (resSimStart.result):
        print 'Simulator could not start value:', resSimStart
    elif 1 ==  (resSimStart.result):
        print 'Simulator started value:',resSimStart
    else :# if 0
        print 'Simulator already running value:',resSimStart
    #time.sleep(3) #wait for node and topics to come up
    # subscibe service    
    srvObHandle = rospy.ServiceProxy('/vrep/simRosGetObjectHandle',simRosGetObjectHandle)
    srvPose = rospy.ServiceProxy('/vrep/simRosGetObjectPose',simRosGetObjectPose)
    srvSetPose = rospy.ServiceProxy('/vrep/simRosSetObjectPose',simRosSetObjectPose)
    srvSetPosition = rospy.ServiceProxy('/vrep/simRosSetObjectPosition',simRosSetObjectPosition)
    srvGetJointMatrix = rospy.ServiceProxy('/vrep/simRosGetJointMatrix',simRosGetJointMatrix)

    #Publish topics
    fkikpub = rospy.Publisher("setFkIk",std_msgs.msg.String,queue_size = 1)
    gripperPub = rospy.Publisher("gripper_controller/position_command",std_msgs.msg.String,queue_size = 1)
    setTargetPub = rospy.Publisher("cmd_vel_SetTarget",std_msgs.msg.Int32MultiArray,queue_size =1)
    cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
    redBox1 = "BoxTF_red1"
   # tf_sub = TransformListener()
    
    time.sleep(1) #wait for node and topics to come up
    
    # define some relative positions
    pickup1={0,-14.52*math.pi/180,-70.27*math.pi/180,-95.27*math.pi/180,0*math.pi/180}
    pickup2={0,-13.39*math.pi/180,-93.91*math.pi/180,-72.72*math.pi/180,90*math.pi/180}
    pickup3={0,-14.52*math.pi/180,-70.27*math.pi/180,-95.27*math.pi/180,90*math.pi/180}
    platformIntermediateDrop={0,16*math.pi/180,52*math.pi/180,73*math.pi/180,0*math.pi/180}
    #platformDrop1={0,54.33*math.pi/180,32.88*math.pi/180,35.76*math.pi/180,0*math.pi/180}--{0,-0.4,0.2}
    #platformDrop2={0,40.74*math.pi/180,45.81*math.pi/180,59.24*math.pi/180,0*math.pi/180}--{0,-0.32,0.2}
    #platformDrop3={0,28.47*math.pi/180,55.09*math.pi/180,78.32*math.pi/180,0*math.pi/180}--{0,-0.24,0.2}
    #define distances, height and speed
    dist1=0.2
    dropHeight1=0.035
    dropHeight2=0.095
    dropHeight3=0.155
    ikSpeed={0.2,0.2,0.2,0.2}
    ikAccel={0.1,0.1,0.1,0.1}
    ikJerk={0.1,0.1,0.1,0.1}
    fkSpeed={1,1,1,1,1}
    fkAccel={0.6,0.6,0.6,0.6,0.6}
    fkJerk={1,1,1,1,1}
    #setGripperTargetMovingWithVehicle()
    gripperPub.publish("open")
    #setFkMode
    fkikpub.publish("fk")
    #openGripper

    # redBox first pickup:
    #m,angle=getBoxAdjustedMatrixAndFacingAngle(tf_sub,redBox1)
    
    #print m
    #print '\n '
    #print angle
    #setTargetPub.publish(quaternion)
    # service test

    
    #use service
    RedCube_handle = srvObHandle('redRectangle1').handle
    vehicle_handle = srvObHandle('youBot_vehicleTargetPosition').handle
    
    m,angle=getBoxAdjustedMatrixAndFacingAngle(RedCube_handle)
    pose = srvPose(RedCube_handle,-1).pose
   # print pose
    #responde = srvSetPose(vehicle_handle,-1,pose.pose)

    #simSetObjectPosition(vehicleTarget,-1,{m[4]-m[1]*dist1,m[8]-m[5]*dist1,0})

    
    pose = Pose()
    pose.position.x=m[1][0]-m[0][1]*dist1
    pose.position.y=m[2][0]-m[1][1]*dist1
    pose.position.z=0
    pose.orientation.x=0
    pose.orientation.y=0
    pose.orientation.z=angle
  #  responde = srvSetPose(vehicle_handle,-1,pose)
   # responde = srvSetOrintation(vehicle_handle,-1,pose.orientation)
    

    


    #try:
        #handle = rospy.ServiceProxy("vrep/simRosGetObjectHandle","BoxTF_red1")
        #result, pose = rospy.ServiceProxy("/vrep/simRosGetObjectPose",'redRectangle1',-1)
        #print pose
        #set_result = rospy.ServiceProxy("/vrep/simRosSetObjectPose",pose)
        #set_result = rospy.ServiceProxy("/vrep/simRosSetObjectPosition", "youBot_vehicleTargetPosition",-1,{3,2,0})
               
    #    except rospy.ServiceException, e:
     #          print "Service call failed: %s"%e

    
    rospy.spin() # node needs to keep running, so the messages come through
        #start Simulator
        
    time.sleep(5) #just wait, a pause
    rospy.wait_for_service('/vrep/simRosStopSimulation')
    stopSimulation = rospy.ServiceProxy('/vrep/simRosStopSimulation',simRosStopSimulation)
    resSimStop = stopSimulation()  
    
    if -1 ==  (resSimStop.result):
        print 'Simulator could not b stoped value:', resSimStop
    else :# if 0
        print 'Simulator stopped value:',resSimStop
    print('Program ended')



coppelia
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: changing values

Post by coppelia »

Hello,

we cannot review code from users, mainly also because in 95% of the time, the bug lies on the user side and we cannot afford to spend that much time. If you believe there is a bug somewhere, then please narrow it down to the maximum (i.e. the most simplistic situation where the error appears), then we can have a look at it.

Here you have the code for simGetObjectMatrix:

Code: Select all

simInt simGetObjectMatrix_internal(simInt objectHandle,simInt relativeToObjectHandle,simFloat* matrix)
{
    C_API_FUNCTION_DEBUG;

    if (!isSimulatorInitialized(__func__))
        return(-1);

    IF_C_API_SIM_OR_UI_THREAD_CAN_READ_DATA
    {
        if (!doesObjectExist(__func__,objectHandle))
            return(-1);
        C3DObject* it=App::ct->objCont->getObject(objectHandle);
        if (relativeToObjectHandle==sim_handle_parent)
        {
            relativeToObjectHandle=-1;
            C3DObject* parent=it->getParent();
            if (parent!=NULL)
                relativeToObjectHandle=parent->getID();
        }
        if (relativeToObjectHandle!=-1)
        {
            if (!doesObjectExist(__func__,relativeToObjectHandle))
                return(-1);
        }
        C3DObject* relObj=App::ct->objCont->getObject(relativeToObjectHandle);
        C7Vector tr;
        if (relObj==NULL)
            tr=it->getCumulativeTransformationPart1();
        else
        {
            C7Vector relTr(relObj->getCumulativeTransformation());
            tr=relTr.getInverse()*it->getCumulativeTransformationPart1();
        }
        tr.getMatrix().copyToInterface(matrix);
        return(1);
    }
    CApiErrors::setApiCallErrorMessage(__func__,SIM_ERROR_COULD_NOT_LOCK_RESOURCES_FOR_READ);
    return(-1);
}
Cheers

Elmo
Posts: 31
Joined: 13 Oct 2016, 16:34

Re: changing values

Post by Elmo »

hello,

the question/problem is simple why send simRosGetObjectPose always different values.
in the tower of hanoi example I always get the same value from simGetObjectPosition.
also the two values differ a lot between simRosGetObjectPose and simGetObjectPosition from the same value. as you can see in the Output code above.
Is that normal? Or why do I always get different values with simRosGetObjectPose and not with simGetObjectPosition?

Thank you very much

simGetObjectPosition always show
position redbox: 0.74999976158142 -1.3866442216681e-08 0.14999997615814
position vehilce: -6.8725348683074e-06 0.16641589999199 -0.00040899217128754

simRosGetObjectPose
redbox: 0.749999761581 6.53132170569e-09 0.149999976158
vehicle: -6.8725348683074e-06 0.16641589999199 -0.00040899217128754

2. run
redbox: [0.7499819993972778, -5.401298403739929e-05, 0.1499999463558197]
vehicle: [2.006388967856765e-06, 0.1662178784608841, 1.4901161193847656e-08]

coppelia
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: changing values

Post by coppelia »

Ah ok, I now understand. Have you looked at the exponent the number has? The number is very small, basically one is 0.0000000138 meters, the other is 0.000054 meters. The difference between both is negligible.
At the same time, an object that is simulated by the physics engine will not always stay exactly in place, even if it should. This also depends on the engine you are using.

Cheers

Elmo
Posts: 31
Joined: 13 Oct 2016, 16:34

Re: changing values

Post by Elmo »

yes true, I only wondered why the vrep intern function simGetObjectPosition always has the same numbers for the cube and vehicle, while the ROS version differs for the red box every time. The position for the vehicle itself is more stable instead. I was just wondering why.
In my naiv thoughts I think intern the simRosGetObjectPose will use the mechanismns of simGetObjectPosition.
But well if thats normal I'm fine with that

merci vielmals

Post Reply